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ABSTRACT 


For  an  autonomous  mobile  robot  to  navigate  in  an  unknown  environment,  it  is 
essential  to  know  the  location  of  the  robot  on  a  real-time  basis.  Finding  position  and 
orientation  of  a  mobile  robot  in  a  world  coordinate  system  is  a  problem  in  localization. 
Dead-reckoning  is  commonly  used  for  localization,  but  position  and  orientation  errors 
from  dead-reckoning  tend  to  accumulate  over  time.  The  objective  of  this  thesis  is  to 
develop  a  feature-based  localization  method  that  allows  a  mobile  robot  to  re-calibrate  its 
position  and  orientation  by  automatically  selecting  wall-like  features  in  the  environment. 

In  this  thesis,  the  selection  of  features  is  accomplished  by  applying  the  Hough 
transform  to  sonar  data.  The  Hough  transform  makes  it  possible  to  select  the  optimal 
feature  (the  longest  wall,  in  this  case)  without  finding  all  possible  line  segments  from  the 
sonar  data.  A  least-square  line  fitting  method  is  then  employed  to  construct  a  model  of 
the  line  segment  that  represents  the  feature  selected  by  the  Hough  transform.  The 
algorithm  developed  was  tested  using  synthetic  and  real  sonar  data.  Experimental  results 
demonstrated  the  effectiveness  of  the  proposed  localization  methods. 
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I.  INTRODUCTION 

During  the  1970s  and  early  1980s,  research  in  robotics  was  mainly  focused  on 
robotic  manipulators  and  their  applications  in  manufacturing  automation.  In  the  last  ten 
years,  there  has  been  a  growing  interest  in  autonomous  mobile  robots,  owing  to  their 
potential  application  in  a  wide  range  of  operations.  HelpMate,  an  autonomous  mobile 
robot  developed  by  Transition  Research  Corporation  in  Danbury,  CT,  is  being  utilized  in 
a  number  of  hospitals  for  delivering  food  and  medicine  to  patients'  rooms.  With  constant 
improvement  in  their  intelligence,  coupled  with  a  decline  in  their  costs,  autonomous 
mobile  robots  are  expected  to  find  their  way  into  schools  for  delivering  books  from  a 
library,  and  into  homes  for  vacuuming  floors. 

Autonomous  mobile  robots  have  many  military  applications  as  well.  Equipped 
with  proper  detection  sensors,  mobile  robots  can  be  deployed  to  search  for  land  mines  (in 
foreign  battlefields)  and  unexploded  ordnance  (in  domestic  lands  previously  utilized  for 
ammunition  storage  and  firing  ranges,  e.g.,  Fort  Ord).  Clearly,  the  use  of  autonomous 
mobile  robots  greatly  reduces  the  danger  to  the  personnel  involved  in  such  operations. 

In  addition  to  safety,  mobile  robots  offer  other  advantages  as  well.  Humans  have 
a  limited  attention  span.  Searching  a  large  field  such  as  one  at  Fort  Ord  is  a  repetitive  and 
tiresome  task.  The  accuracy  of  searching  by  humans  will  inevitably  depend  on  the  degree 
of  concentration.  Robotic  systems  are  ideally  suited  for  such  repetitive  and  tedious  tasks 
(aside  from  safety  concerns)  because  they  are  not  subject  to  fatigue  and  degradation  in 
attention  and  thus  provide  uniform  accuracy  in  searching  tasks.  As  well  as  being 
deployed  in  battlefields,  another  application  of  mobile  robots  is  in  maintaining  security  by 
patrolling  areas  such  as  offices,  warehouses,  airports,  and  other  facilities.  Here  again, 
robots  are  well  suited  since  they  are  not  adversely  influenced  by  the  repetitiveness  and 
monotony. 

An  important  issue  associated  with  the  autonomous  operation  of  a  mobile  robot  is 
localization,  i.e.,  finding  real-time  position  and  orientation  of  the  robot  in  a  certain 
coordinate  system.  When  operating  in  outdoor  environments,  the  availability  of  GPS 
makes  the  localization  problem  significantly  simpler.  But  GPS  is  not  a  universal  solution 
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for  all  applications.  Obviously,  a  mobile  robot  that  operates  in  an  indoor  environment  is 
not  able  to  receive  GPS  signals.  Likewise,  a  mobile  robot  that  operates  near  buildings 
and  trees  may  not  be  able  to  establish  connections  with  four  satellites  simultaneously. 
Even  if  GPS  were  available,  its  accuracy  may  not  be  sufficient  for  some  applications. 

Dead-reckoning  is  a  widely  used  method  for  self-localization  for  mobile  robots. 
This  method  computes  position  and  orientation  by  integrating  wheel  displacements  that 
are  typically  measured  using  optical  encoders.  For  example,  an  automobile  odometer 
uses  dead-reckoning  to  keep  track  of  distance  traveled.  Due  to  the  nature  of  integration, 
position  and  orientation  errors  accumulate  over  time.  The  longer  a  mobile  robot  moves, 
the  poorer  its  localization  accuracy.  Dead-reckoning  errors  are  classified  into  systematic 
and  non-systematic  types  (Feng  et  al.,  1996).  Systematic  errors  are  due  to  such  things  as 
unequal  wheel  diameters,  differences  in  actual  and  measured  wheel  diameters  and 
wheelbase,  wheel  misalignment,  and  encoder  errors.  Non-systematic  errors  are  due  to 
external  factors  such  as  uneven  floors  and  wheel  slippage  on  the  floor.  In  most 
applications,  wheel  slippage  is  the  main  cause  of  dead-reckoning  errors. 

To  enable  an  autonomous  mobile  robot  to  operate  over  an  extended  period  of 
time,  there  is  a  clear  need  to  correct  or  reduce  dead-reckoning  errors.  One  general 
approach  to  correcting  dead-reckoning  errors  is  to  provide  a  mobile  robot  with  the  ability 
to  recognize  physical  features  or  landmarks  in  its  environment  as  it  moves  along  and  to 
calibrate  itself  with  respect  to  these  features  when  it  arrives  at  the  previously  traveled  area 
again.  In  this  thesis,  the  calibration  of  mobile  robot  coordinates  in  a  laboratory 
environment  will  be  investigated.  The  objective  is  to  develop  algorithms  that  allow  a 
mobile  robot  to  automatically  select  existing  laboratory  features  such  walls  or  benches  to 
calibrate  its  position  and  orientation. 


II.  APPROACH 
A.         LITERATURE  SURVEY 

As  stated  in  the  previous  chapter,  this  thesis  investigates  localization  of  mobile 
robots.  The  problem  of  localization  has  been  actively  studied  by  many  researchers  in  the 
literature.  One  approach  assumes  the  availability  of  a  world  map.  As  a  mobile  robot 
moves  in  a  previously  mapped  environment,  it  attempts  to  recognize  features  in  the 
environment  and  to  find  a  recognized  feature  in  the  map.  If  a  feature  is  found  in  the  map, 
the  robot's  position  and  orientation  relative  to  this  feature  is  calibrated  using  the  map.  If  a 
feature  is  not  found  in  the  map,  the  map  is  updated  by  adding  this  feature.  Representative 
work  of  this  approach  includes  Drumheller  (1987),  Crowley  (1989),  and  Gonzalez  et  al. 
(1994).  When  using  this  approach,  one  must  solve  two  main  problems:  feature 
recognition  and  correspondence  (or  matching).  The  first  problem  is  concerned  with  how 
to  recognize  a  feature  (wall,  corner)  using  vision,  sonar,  laser  range  finder,  or  other 
sensor.  The  second  one  is  concerned  with  how  to  match  a  recognized  feature  with  the 
corresponding  one,  if  it  exists,  in  the  map.  If  a  world  map  is  not  initially  available,  it  may 
be  constructed  using  a  dynamic  map-building  method  (Vandorpe  et  al.,  1996)  and  by 
exploration  of  unknown  environments  (Bauer  and  Rencken,  1995). 

Since  the  recognition  of  environmental  features  is  a  difficult  problem,  some 
researchers  proposed  to  use  beacons  (Leonard  Durrant-Whyte,  1991).  Beacons  are  a 
special  type  of  targets  that  can  reliably  be  recognized  by  appropriate  sensors.  The  use  of 
beacons  significantly  simplifies  the  recognition  problem,  but  it  requires  that  beacons  be 
installed  in  the  environment  where  a  mobile  robot  is  to  be  deployed.  This  approach  may 
not  be  applicable  in  certain  applications  where  the  installation  of  beacons  is  not  feasible 
or  not  permitted. 

Various  sensors  have  been  used  for  the  purpose  of  localization.  Sonar  is  widely 
used  for  localization  due  to  its  speed  and  simplicity  (Drumheller,  1987;  Crowley,  1989; 
Kuc  and  Viard,  1991;  Adams  et  al.,  1994).  Other  sensors  used  for  localization  include 
laser  range  finders  (Vandorpe  et  al.,  1996),  vision  (Yeh  and  Kriegman,  1995;  Yang, 
1995),  optical  gyroscopes  (Komoriya  and  Oyama,  1994),  and  a  combination  of  sonar  and 
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infrared  (Curran  and  Kyriakopoulos,  1993). 

B.         PROBLEM  STATEMENT  AND  PROPOSED  APPROACH 

This  thesis  studies  the  localization  problem  of  a  Nomad  200  mobile  robot 
(Nomadic  Technologies,  1993).  The  mobile  robot  is  equipped  with  16  sonar  sensors  that 
are  equally  spaced  on  a  ring-like  structure.  Two  adjacent  sensors  are  separated  by  22.5°. 
The  robot  uses  two  coordinate  systems  for  navigation:  a  local  robot  coordinate  system 
and  a  global  world  coordinate  system.  The  world  coordinate  system  is  an  inertial  system 
in  which  the  position  and  orientation  of  the  robot  is  represented.  The  robot  coordinate 
system  is  attached  to  the  robot  and  moves  with  it.  When  the  robot  is  first  turned  on,  the 
two  coordinate  systems  are  identical.  The  position  and  orientation  of  the  robot  is  at 
(0,0,0).  The  orientation  of  the  robot  (and  of  the  world  coordinate  system)  is  very  much 
arbitrary.  Therefore,  it  is  highly  desirable  to  line  up  the  axes  of  the  world  coordinate 
system  with  a  wall  or  other  major  feature  so  that  it  will  be  easier  to  specify  the  goal 
position  and  orientation  of  the  robot.  A  current  practice  is  to  lift  up  the  robot  and 
carefully  position  itsx-axis  or  v-axis  along  a  wall,  which  is  highly  impractical.  One 
objective  of  this  thesis  is  to  enable  a  mobile  robot  to  automatically  establish  a  world 
coordinate  system  whose  axes  are  parallel  to  major  features  in  the  environment.  To  this 
end,  a  supplementary  objective  is  to  develop  a  localization  algorithm  which  is  based  on  a 
natural  feature  in  the  laboratory  environment  using  the  sonar  sensors.  To  achieve  these 
two  objectives,  it  is  necessary  to  solve  the  following  problems: 

(1)  How  to  select  a  feature  from  the  environment  that  is  the  best  for  localization 
purposes,  and 

(2)  How  to  recognize  the  selected  feature  using  the  sonar  data. 

Walls  and  wall-like  features  such  as  a  long  bench  are  good  features  for  calibration 
and  localization.  Walls  can  be  represented  by  line  segments.  Due  to  the  noise  nature  and 
low  resolution  of  sonar  data,  short  line  segments  are  more  error-prone  for  recognition. 
Therefore,  it  is  desirable  to  select  the  longest  wall  in  the  robot's  environment  as  a  feature. 
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The  previous  work,  e.g.,  Vandorpe  et  al.  (1996),  attempts  to  first  solve  the  recognition 
problem  and  then  solve  the  selection  problem.  Linear  regression  is  used  to  recognize  all 
possible  line  segments.  This  requires  evaluation  of  the  relative  positions  of  sonar  data 
points  to  determine  if  they  belong  to  a  line  segment.  Furthermore,  data  points  are 
assumed  to  be  gathered  in  a  geometrically  sequential  order.  A  newly  acquired  data  point 
is  only  tested  against  the  present  line.  If  it  does  not  fit  into  the  present  line,  a  new  line  is 
started. 

In  this  thesis,  the  selection  problem  and  the  recognition  problem  are  solved 
simultaneously.  A  line  segment  corresponding  to  the  longest  wall  is  extracted.  This  is 
made  possible  by  using  the  Hough  transform  (Hough,  1962;  Duda  and  Hart,  1972). 
While  the  Hough  transform  is  widely  used  in  computer  vision  (Ballard  and  Brown,  1982; 
Haralick  and  Shapiro,  1993),  its  use  in  localization  on  sonar  sensor  data  is  new. 

The  mathematical  basis  for  the  Hough  transform  lies  in  point-line  and  point-curve 
transformations,  or  plane  transformations.  It  converts  the  problem  of  finding  collinear 
points  into  a  problem  of  finding  concurrent  lines  (i.e.,  lines  passing  through  a  single 
point).  Each  point  in  the  original  data  space,  or  coordinate  space,  is  transformed  into  a 
straight  line  or  a  sinusoidal  curve  in  the  parameter  space.  By  using  this  method,  there  is 
no  need  to  recognize  all  line  segments  to  select  the  longest  one.  The  method  is  able  to 
find  all  the  points  that  form  the  longest  line  segment.  Furthermore,  this  method  does  not 
require  that  data  points  be  in  any  order. 

In  this  work,  a  'picture'  or  image  is  obtained  by  collecting  sonar  data  over  a  finite 
interval  using  the  Nomad  200  mobile  robot.  The  sonar  returns  plotted  on  the  coordinate 
plane  represent  the  environment  of  obstacles  that  the  robot  detects.  A  wall,  or  other  wall- 
like surface,  appears  as  a  line  in  this  picture,  and  the  problem  is  to  identify  collinear 
points  within  the  data  collected.  This  can  be  done  by  processing  the  sonar  'picture'  via 
implementation  of  the  Hough  transform  to  determine  the  existence  of  prominent  lines. 
The  longest  line  is  extracted  and  least-square  line-fitting  is  applied  to  determine  the  line 
segment  that  will  be  used  as  a  reference  for  localization. 


The  next  chapter,  Chapter  III,  discusses  parametric  representations  of  a  line,  the 
Hough  technique  and  its  implementation  as  a  computer  algorithm,  and  the  line  estimation 
algorithm.  In  Chapter  IV,  the  results  of  testing  the  algorithms  developed  with  both 
synthetic  and  real  sonar  data  are  presented.  The  findings  and  issues  of  the  experimental 
data  are  discussed  in  Chapter  V,  and  concluding  remarks  and  recommendations  for  future 
work  are  given  in  Chapter  VI. 


III.  THEORY  AND  IMPLEMENTATION 

A.         PARAMETRIC  REPRESENTATION  OF  A  LINE  -  PLANE 
TRANSFORMATIONS 

A  straight  line  in  the  x-y  coordinate  plane  can  be  described  completely  by  just  two 
parameters,  the  slope  and  the  y-intercept.  The  familiar  parameterization  is: 


y  =  m0x  +  b0 


(1) 


where  mo  is  the  slope  and  bo  is  the  y-intercept.  This  line,  when  plotted  in  the  m-b 
parameter  plane,  becomes  a  single  point.  It  can  be  seen  that,  given  a  point  (xo,yo),  the  set 
of  all  lines  through  (xo,yo),  each  of  which  is  specified  by  some  (m,b)  pair,  will  be 
transformed  into  a  straight  line  in  the  parameter  (m-b)  plane  (see  Figure  1). 


(a) 


(m'.b') 


(b) 


-^->- 


(mo .  bo) 


m 


Figure  1.  Point-line  transformations,  (a)  A  point  with  lines  of  various 
(m  ,b)  parameters  through  it  transforms  into  a  straight  line  in  the 
parameter  plane,  (b)  Collinear  points  transform  into  straight  lines 
intersecting  at  a  single  point  in  the  parameter  plane. 
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This  is  evident  from  the  transformed  equation  which  indicates  the  linear  relationship 
between  m  and  b: 


b  =  —x0m+y0 


(2) 


It  can  further  be  seen  that,  if  points  lying  in  line  with  each  other  in  the  coordinate  (x-y) 
plane  are  transformed  into  their  respective  lines  in  the  parameter  plane,  those  lines  in  the 
parameter  plane  will  have  a  single  intersection  point.  The  (m,b)  coordinates  of  this 
intersection  point  are  exactly  the  slope  and  intercept  of  the  line  through  the  points  in  the 
coordinate  plane.  Note  that  in  using  this  parameterization,  there  exists  a  singularity  in 
that  the  slope,  the  independent  variable  in  this  case,  is  unbounded  (i.e.,  when  a  line  in  the 
coordinate  plane  is  parallel  to  the  y-axis,  the  slope  goes  to  infinity).  Figure  1  illustrates 
the  point-line  relationships. 

As  presented  by  Duda  and  Hart  (1972),  a  more  convenient  way  to  describe  a 
straight  line  is  by  using  the  normal  parameterization  (Figure  2): 


r0  =  xcos60  +  y  sin#0 


(3) 


ro  =  x  cos  8o+y  sin  8q 


Figure  2.  Normal  parameterization. 


Here,  a  line  in  the  coordinate  plane  is  described  completely  by  the  two  parameters,  #0,  the 
angle  of  its  normal  through  the  origin,  and  r0,  its  distance  from  the  origin. 

Following  the  point-line  transformation  discussion  above,  given  a  point  (jtojo)  in 
the  coordinate  plane,  the  set  of  lines  through  (xojo),  each  of  which  is  specified  by  some 
(0 ,  r)  pair,  will  be  transformed  into  a  sinusoidal  curve  in  the  parameter  (9-r)  plane  (see 
Figure  3).  The  curve  has  the  equation: 


r  -  x0  cosd  +  y0  sin  6 


(4) 


(a) 


(e'",r; 


(b) 


Figure  3.  Point-curve  transformations,  (a)  A  point  with  lines  of 
various  (8  ,r )  parameters  passing  through  it  transforms  into  a 
sinusoidal  curve  in  the  parameter  plane,  (b)  Collinear  points  transform 
into  curves  intersecting  at  a  single  point  in  the  parameter  plane. 


In  this  case,  if  points  lying  in  line  with  each  other  in  the  coordinate  (x-y)  plane  are 
transformed  into  the  respective  sinusoidal  curves  in  the  parameter  plane,  those  curves  in 


the  parameter  plane  will  have  a  single  intersection  point.  The  (6  ,  r)  coordinates  of  this 
intersection  point  are  exactly  the  angle  (or  orientation)  of  the  normal  and  the  distance 
from  the  origin  of  the  line  through  the  points  in  the  coordinate  plane.  Figure  3  illustrates 
the  point-curve  relationships.  Properties  of  point-curve  transformations  are  described  in 
Duda  and  Hart  (1972). 

B.         HOUGH  TRANSFORM  TECHNIQUE 

The  Hough  technique  employs  the  point-curve  transformation  discussed  above.  It 
is  based  on  the  premise  that  collinear  points  in  the  coordinate  (image)  plane  will  be 
transformed  into  curves  with  a  single  point  of  intersection  in  the  parameter  plane.  To  find 
a  line  in  the  image,  then,  the  procedure  is  to  transform  the  points  on  the  sonar  image  into 
the  corresponding  curves  in  the  parameter  plane,  and  then  to  locate  the  intersections  of 
curves  to  determine  the  presence  of  lines. 

Analyzing  the  parameter  plane  for  precise  intersection  points  would  clearly  be 
tedious  and  inefficient.  Per  Duda  and  Hart  (1972),  if  the  parameter  space  is  discretized 
into  a  two-dimensional  grid,  then  each  cell  in  the  grid  represents  a  region  in  which 
intersecting  curves  will  correspond  to  a  set  of  nearly  collinear  points  since  the  region 
encompasses  a  range  of  slopes  and  distances.  The  resolution  of  the  grid  would  be  based 
upon  the  how  much  'scatter'  is  permitted  for  the  points  used  to  estimate  the  line. 

If  each  cell  in  the  parameter  plane  grid  is  examined  to  determine  a  set  of  curves 
that  intersect  (pass  through)  it,  then  the  set  of  the  corresponding  coordinate  points  will 
provide  an  approximate  line  in  the  image  plane.  The  general  procedure  is  as  follows: 

1 .  For  a  given  point,  generate  a  6-r  curve  plotted  on  the  parameter  plane  grid. 

2.  Note  the  cells  that  the  curve  crosses. 

3.  Repeat  Steps  1  and  2  for  every  point. 

4.  Count  the  number  of  crossings  in  each  cell. 

5.  Recover  the  points  whose  curves  contributed  to  the  total  of  each  cell. 

6.  Estimate  a  line  for  each  set  of  points. 
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Obviously,  some  kind  of  criterion  must  be  imposed  for  selecting  which  cells 
should  be  considered  for  further  examination  since  any  cell  containing  two  or  more 
curves  should  give  a  possible  line  in  the  image  (a  line  can  be  drawn  between  any  two 
points).  It  would  be  neither  desirable  nor  practical  to  examine  every  cell,  especially  as 
every  pair  of  points  in  the  image  would  imply  the  existence  of  lines,  most  of  which  are 
probably  erroneous.  Erroneous  lines  can  also  result  when  more  than  two  points  in  the 
image  coincidentally  lie  in  a  line,  but  represent  no  physical  entity  in  the  environment. 
While  erroneous  lines  are  unavoidable,  the  likelihood  can  be  somewhat  reduced  by 
setting  some  threshold  for  the  minimum  number  of  points  that  will  be  considered  to 
render  a  line. 

In  this  application,  the  only  lines  of  interest  will  be  those  that  most  likely 
represent  a  significant  wall.  The  assumption  can  be  made  that  the  sonar  returns  from 
such  a  wall  will  be  frequent,  i.e.,  that  the  greater  the  number  of  points  contributing  the 
line,  the  more  likely  that  line  represents  a  wall.  The  assumption  can  also  be  made  that  the 
line  with  the  most  points  is  likely  to  be  the  longest  line  since  the  frequency  of  the  sonar 
beams  intercepting  an  object  of  sizeable  extent  is  likely  to  be  greater.  Therefore,  the  line 
with  the  most  points  is  selected  as  a  reference  wall. 

C.         HOUGH  TRANSFORM  CODE 

Computer  programs  were  developed  to  implement  the  Hough  transform 
technique.  The  method  is  executed  in  three  parts:  (1)  data  collection,  (2)  the  Hough 
algorithm,  and  (3)  the  line  estimation  algorithm.  A  'shell  program,'  collect _data.c,  reads 
in  the  data  from  the  robot  and  then  invokes  the  programs  hough.c  and  linest.c 
successively.  The  program  hough.c  is  the  main  algorithm  —  it  implements  the  Hough 
transform  and  extracts  the  possible  lines  present  in  the  image.  The  program  linest.c 
performs  a  line-fitting  using  the  points  provided  by  hough.c  for  the  extracted  line.  The 
present  code  finds  multiple  possible  lines  in  a  given  set  of  data,  and  estimates  the  longest 
line.  For  initial  testing,  the  shell  program  synthjiata.c  was  used  to  read  an  input  file  of 
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synthetic  data  instead  of  collecting  sonar  data  with  the  robot.  All  programs  are  included 
as  appendices. 

1.  Data  Collection 

In  collect _data.c,  sonar  information  is  collected  by  the  Nomad  200  robot.  A  real 
world  scenario  might  be  to  have  the  patrolling  robot  come  to  a  stop,  scan  its  environment 
and  survey  the  situation.  Hence,  the  robot  was  programmed  to  remain  stationary  in  its 
coordinate  position,  while  its  turret  was  rotated  through  some  nominal  angle  to  collect 
sonar  readings.  The  coordinates  of  the  sonar  reflections  from  obstacles  in  the  vicinity  of 
the  robot  were  then  stored  to  be  processed  subsequently  by  the  program  hough.c. 

Data  collection  will  have  some  consequence  on  the  spread  of  the  data  points.  This 
is  an  important  consideration  in  finding  an  appropriate  reference  wall.  Under  the 
assumption  that  a  line  containing  the  most  points  is  also  the  longest,  and  likely  to  be  a 
wall,  there  is  still  the  possibility  of  finding  a  non-contiguous  line.  For  example,  a  wall 
with  a  doorway  will  look  like  a  line  with  a  segment  missing,  or  objects  in  the  room  that 
are  situated  in  line  with  each  other  will  look  like  a  line  of  several  segments.  But  neither 
of  these  are  appropriate  to  use  as  a  reference  surface  for  the  purpose  of  calibration. 

The  Nomad  200  interface  software  is  configured  such  that  the  ring  of  16  sonar 
sensors  are  cycled  through  consecutively,  giving  readings  which  cover  all  360°  around  the 
robot.  The  distance  between  the  sonar  returns  for  two  adjacent  sensors  grows  larger  for 
obstacles  at  distances  farther  away  from  the  robot,  and  the  data  points  collected  grow 
farther  apart.  In  order  to  obtain  data  points  closer  together,  the  robot  must  rotate  through 
some  angle  less  than  the  angular  separation  between  two  adjacent  sensors  (see  Figure  4). 

In  the  case  of  this  work,  the  sonar  data  was  collected  at  7.5°  rotations  of  the 
robot's  turret.  Since  the  separation  angle  between  two  adjacent  sensors  is  22.5°,  three 
cycles  of  sonar  readings  can  be  collected  before  the  data  becomes  redundant.  Thus,  a 
tighter  resolution  of  data  can  be  obtained,  as  illustrated  in  Figure  4.  The  16  sensors  were 
read  at  each  of  the  orientations  —  initial  orientation,  initial+7.5°,  and  initial+15°  --  to 
provide  a  total  of  48  points. 
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(a) 


(b) 


Figure  4.  Separation  of  data  points,  (a)  Sonar  readings  spread  farther  apart  with 
distance,  (b)  Additional  points  provide  information  that  might  otherwise  be  missed. 


2.  Hough  Algorithm 

The  mechanics  of  the  Hough  algorithm,  hough.c,  can  be  viewed  as  follows: 
transform  the  points  in  the  sonar  image  into  curves  in  the  parameter  plane;  superimpose  a 
two-dimensional  grid  over  the  parameter  plane;  construct  a  two-dimensional  accumulator 
array  whose  elements  (hereinafter  referred  to  as  'bins')  correspond  to  the  cells  in  the 
parameter  plane  grid;  for  each  point,  note  each  cell  that  its  curve  crosses;  increment  the 
corresponding  bin  in  the  accumulator  array  and  keep  track  of  the  point;  and,  finally,  for 
each  bin  of  interest,  recover  the  points  associated  with  it.  Figure  5  illustrates  the  steps. 

a.  Defining  the  Grid  and  the  Accumulator  Array 

The  resolution  of  the  grid  is  set  by  the  size  of  the  increments,  6-resolution  and  r- 
resolution,  which  are  chosen  to  discretize  the  6  and  r  parameters.  If  the  resolution  is  too 
coarse,  then  the  possible  lines  found  will  have  to  be  estimated  from  points  that  are  more 
widely  dispersed.  If  the  resolution  is  too  fine,  perhaps  finer  than  the  precision  of  the 
measurement,  then  the  same  line  will  be  found  by  several  cells  that  are  grouped  together. 
The  difficulty  is  in  allowing  enough  tolerance  in  the  data  so  that  a  line  (i.e.,  a  reference 
wall)  can  be  estimated  as  accurately  as  possible. 
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Figure  5.  Hough  algorithm  steps,  (a)  Sonar  picture,  (b)  Plane 
transform,  (c)  Parameter  grid    (d)  Accumulator  array. 


Since  the  accumulator  array  size  has  to  be  finite,  a  consideration  must  be  made 
regarding  the  limits  of  the  parameters.  The  convention  that  9  is  defined  in  the  interval 
[0,180°]  while  r  can  take  on  both  positive  and  negative  values  was  used.  A  negative 
value  for  r  indicates  that  the  line's  normal  to  the  origin  falls  in  the  third  or  fourth 
quadrant  of  the  coordinate  plane,  and  its  angle,  measured  from  the  x-axis,  is  that  of  the 
normal  extended  into  the  first  or  second  quadrant.  Figure  6  illustrates  this  definition. 
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Figure  6.  Definition  of  6  and  r. 


An  estimate  of  a  bound  for  r  is  needed  to  limit  the  accumulator  array  size  and  can 
be  established  using  the  data.  Let  R,  be  the  distance  from  the  origin  of  any  point  (Xi,yi): 


*,=<ff 


+y; 


(5) 


Note  that  this  is  not  the  same  as  r,  the  distance  from  the  origin  to  an  arbitrary  line,  defined 
in  Equation  (4).  It  can  be  shown  that  the  distance  Rq  to  a  point  (jcoJo)  is  the  largest 
distance  that  can  be  obtained  for  any  line  drawn  through  that  point.  Recall  that  the 
distance  from  a  point,  such  as  the  origin,  to  a  line  is  the  length  of  a  line  segment  between 
the  point  and  the  line  which  is  perpendicular,  or  normal,  to  the  line.  This  normal  line 
segment  is  the  shortest  line  segment  that  can  be  drawn  from  the  original  point  to  any  other 
point  on  the  line.  For  a  point  (xo,yo)  in  the  coordinate  plane,  there  exists  only  one  line 
through  the  point  whose  distance  r0  is  equal  to  the  distance  Ro  to  the  point.  This  is  the 
line  whose  normal  line  segment  coincides  with  the  line  segment  between  the  origin  and 
(*o,.yo)-  Furthermore,  every  other  line  through  the  point  (x0,vo)  nas  its  own  unique  normal 
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line  segment  which  does  not  coincide  with  the  line  segment  between  the  origin  and 
(x0,yQ),  and  is,  by  definition,  shorter  than  R0.  Therefore,  R0  is  the  largest  distance  that  can 
be  obtained  for  any  line  drawn  through  the  point  (x0,Vo)-  A  logical,  and  conservative, 
estimate  of  the  maximum  extent  of  the  area  covered  by  the  image,  then,  is  the  maximum 
absolute  value  expected  for  R,.  Let  this  estimate  be  Rmax,  where 

«raax=max(/?l)  .  (6) 

Thus,  r  is  bounded  by  [-Rmax,+Rmax\- 

In  order  to  define  a  grid  with  cells  of  uniform  size,  /?max  must  be  wholly  divisible 
by  the  r-resolution  which  discretizes  r.  The  range  for  r  can  be  adjusted  to  [-rmax^+fmaxL 
where  rmax  is  the  closest  multiple  of  r-resolution  larger  than  /?max- 

The  size  of  the  accumulator  array  is  M  columns  by  TV  rows,  and  each  bin  is 
indexed  by  its  row  and  column.  The  row  and  column  indices  must  be  non-negative 
integers.  The  number  of  columns  M  is  simply: 


180° 

M  = .  (7) 

6    resolution 


Since  r  ranges  from  -rmax  to  +rmax  the  number  of  rows  N  is 


xr  _  [        max        V        max  ^J "^max ^m 

r  resolution         r   resolution 


The  grid  plane  can  be  translated  directly  into  the  accumulator  array.  The  column 
index  is  read  directly  from  the  0-axis  marked  off  in  increments  of  6-resolution.  To  obtain 
the  row  index,  the  r-axis  must  be  shifted  down  by  an  amount  rmax  so  that  the  zero  position 
is  at  -rmax-  The  row  index  is  read  from  the  shifted  axis  marked  off  in  increments  of  r- 
resolution.  The  translation  is  shown  in  Figure  7  . 
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Figure  7.  Translation  of  r  into  row  number,  (a)  Bounds  for  r.  (b)  Bounds 
for  discretized  r.  (c)  Axis  shifted  to  convert  into  rows,  (d)  Accumulator 
array. 


b.         Discretizing  the  Plane  Transform 

Each  point  in  the  image  is  transformed  into  the  corresponding  sinusoidal  curve, 
and  the  curve  is  mapped  onto  the  parameter  grid.  The  curve  is  generated  by  calculating  r 
for  each  value  of  the  discretized  independent  variable  6  using  Equation  (4).  If  the 
problem  was  simply  to  discretize  r  by  truncating  any  decimal  portion,  then  discretizing 
the  plane  transform  (i.e.,  the  sinusoidal  curve)  would  be  trivial.  This  does  not,  however, 
account  for  sections  where  the  slope  of  the  curve  changes  very  rapidly  such  that  the  curve 
crosses  more  than  one  cell  between  two  consecutive  values  of  0 .  It  is  clear  that  vital 
information  may  be  overlooked. 

Rather  than  tabulating  the  (0 ,  r)  pairs  and  assigning  them  to  the  corresponding 
bins,  a  method  was  devised  which  takes  the  approach  of  examining  the  columns  of  cells 
in  the  grid  instead.  All  of  the  affected  cells  in  each  column  are  then  translated  into  the 
corresponding  bins  in  the  accumulator  array.  Figure  8  illustrates  this  method.  As  the 
curve  is  generated,  r(6 ■)  and  r(6J+\)  are  calculated  forj'=0,. . .,  M-\  (M  is  the  total  number 
of  columns  or  increments  of  6 ).    The  pairs  {dp  r(0j))  and  (0,+i,  r(dJ+\J)  represent  the 
endpoints  of  that  part  of  the  curve  which  falls  in  column),  and  passes  through  one  or 
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more  cells.  Next,  r(6j)  and  r(6j+i)  are  converted  into  the  corresponding  rows,  row(6,)  and 
row(dl+\).  Thus,  the  bins  in  column  y  between  row(0;)  and  row(0J+\)  are  incremented. 
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Figure  8.  Method  of  counting  which  bins  to  increment  for  a  curve. 


c.  Finding  the  Line  and  Points  in  the  Line 

The  iterative  process  of  treating  each  point  in  the  image  includes  assigning  the 
point  to  each  accumulator  array  bin  which  is  affected  by  the  corresponding  plane 
transform  curve.  In  other  words,  the  point  transforms  to  a  parameterized  curve,  the  curve 
intersects  a  series  of  cells  in  the  parameter  plane  grid,  the  cells  translate  into  bins  in  the 
accumulator  array,  and  the  bins  record  information  which  will  need  to  be  recovered.  The 
information  required  is  the  total  number  of  points  in  each  bin  after  all  the  points  have 
been  processed,  and  the  points  associated  with  the  specific  bin(s)  of  interest.  The  total 
number  of  points  will  indicate  the  possible  existence  of  a  line  and,  thus,  a  wall.  The 
associated  points  will  provide  the  data  to  estimate  the  line. 

Counting  the  points  is  a  simple  matter  of  incrementing  each  bin  whenever  it  is 
encountered.  Tracking  the  points,  while  slightly  confusing  to  implement,  is  actually 
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straightforward.  The  desired  product  is  a  list  of  points  that  are  associated  with  the 
accumulator  bin  containing  the  most  points,  but  since  the  specific  bin  cannot  be 
determined  until  all  the  points  have  been  processed,  each  bin  must  be  somehow  linked  to 
all  the  points  that  were  counted  as  part  of  that  bin's  total.  Also,  since  the  same  point 
would  have  been  counted  in  several  bins,  what  results  is  a  network  of  links  that  rapidly 
becomes  quite  intricate. 

The  tracking  procedure  that  was  devised  builds  a  chain  of  structures,  named  point 
structures  (in  C  programming  language),  connected  to  each  accumulator  bin.  A  point 
structure  holds  information  that  identifies  the  point  that  was  counted  and  the  memory 
address  of  another  point  structure  holding  information  about  the  previous  point  counted 
in  the  same  bin.  Hence,  each  point  structure  leads  to  the  previous  point.  In  order  to 
maintain  a  connection  to  the  chain  of  point  structures,  the  accumulator  bin  retains,  in 
addition  to  the  total  point  count,  the  memory  address  of  the  point  structure  containing  the 
last  point,  i.e.,  the  most  recent  point,  that  was  counted.  When  a  bin  A  is  found  to  include 
the  current  point  being  processed,  implementation  of  the  tracking  procedure  is  as  follows: 

1 .  Increment  the  point  total  in  bin  A. 

2.  Create  a  point  structure,  C,  for  the  current  point. 

3.  Store  the  index  of  the  current  point  in  point  structure  C. 

4.  Copy  the  address  of  the  last  point  from  bin  A  into  point  structure  C. 

5.  Store  the  address  of  point  structure  C  in  bin  A . 

The  index  of  the  point  is  stored  instead  of  the  actual  point  coordinates  for  programming 
efficiency.  The  concept  is  illustrated  in  Figure  9:  when  the  current  point,  C,  is 
determined  to  belong  to  a  bin,  A,  the  bin  is  incremented,  the  connection  from  the  bin  to 
the  chain  of  points  is  attached  to  C  (thereby  adding  to  the  chain),  and  the  bin  is  connected 
instead  to  C,  the  last  point  that  has  been  processed.  The  procedure  is  repeated  when 
another  point  is  encountered  that  belongs  in  bin  A. 
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Point  structure  C   ■"" 
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Bin  A 
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Point  index 


List  of  points  in  Bin  A 


First  point  counted 
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Figure  9.  Point-tracking  procedure. 


3.  Line  Estimation 

A  least-square  line-fitting  method  is  used  to  estimate  the  line  from  the  list  of 
points  provided  by  the  Hough  algorithm  for  the  longest  line.  As  in  the  Hough  algorithm 
hough.c,  the  line  estimation  algorithm  linest.c,  based  on  the  algorithm  developed  in 
Albayrak  (1996),  uses  the  normal  parameterization  of  a  line: 


r  =  xcos6  +  vsin# 


(9) 


For  points  (x„y,),  i=\,...,n  where  n  is  the  total  number  of  nearly  collinear  points,  it  is 
desired  to  find  a  unique  solution  for  (0 ,  r )  such  that 
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r  =  xi  cos6  +  y.  sin 9  .  (10) 

Since  the  (x„y,)  are  not  perfectly  in  line,  6  and  r  will  have  some  variation.  The  best 
estimate  of  the  line  will  be  that  which  minimizes  the  variation  in  0  and  r.  Let  S  be  a  cost 
function  defined  as  the  sum  of  the  squares  of  the  residuals,  a  measure  of  the  total 
variation: 

n 

5  =  X<X cos0+y, sin0-r)2  .  (11) 

i=i 

To  minimize  the  variation,  the  cost  function  S  is  differentiated  with  respect  to  each  of  6 
and  r  .  First,  to  solve  for  r  ,  differentiate  S  with  respect  to  r  : 


r)S  n  n 

—  =  0  =  2nr-Yx:cosd-Tyism6  (12) 


so  that 


r  =  —  £(*,.  cos0+y.  sin0)  .  (13) 


2n 


(=i 


Substituting  this  back  into  the  original  function, 


"  1        "  2 

S  =  Y  [xt  cos0+y(.  sin#-  —  ]T  (x.  cosfl+y,.  sin0)]    .  (14) 

i=i  2ft  ,=1 


Differentiating  with  respect  to  0  yields 
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ds 

^  =  O  =  ^,sin(20)  +  2/i2cos(20)  (15) 


with 


Mi=I>:  "5>«  +"^-J (16) 


and 


ft=£(»,y,)-^''^y'  ■  d7) 


Equation  (15)  above  can  be  satisfied  if 

sin(20)  =  -2fi2  (18) 

and 

cos(20)  =  //,  .  (19) 

Therefore,  0  can  be  found  from: 


0  =  -atan2(-2;u2,iu1)  •  (20) 


In  the  linest.c  algorithm,  the  estimated  line,  described  by  its  (0  ,  r  )  parameters,  is  found 
by  computing  ji\  and  fii  from  all  the  points  (jc„v,). 
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IV.  EXPERIMENTAL  RESULTS 

A.         SYNTHETIC  DATA 

The  algorithms  developed  were  initially  tested  using  synthetic  data.  As  mentioned 
previously,  the  shell  program  synthjiata.c  enabled  the  use  of  the  synthetic  input  data 
files  and  invoked  the  algorithms  for  the  Hough  transform  and  the  line  fitting/estimation. 
Because  of  the  highly  iterative  nature  of  the  Hough  algorithm,  a  simple  exercise  using  a 
small  data  set,  cross.data,  was  run  to  observe  how  well  the  whole  code  would  perform. 
Then  two  larger  data  sets,  corner.data  and  corridor. data,  were  tested  to  simulate  possible 
real  world  scenarios.  For  each  data  set,  the  influence  of  the  grid  resolution  was 
examined. 

1.  A  Simple  Exercise:  cross.data 

The  data  set  cross.data  contained  10  points  positioned  in  the  form  of  a  skewed 
cross  (see  Figure  10).  The  short  arm  of  the  cross  contained  four  points,  and  the  long  arm 
contained  seven,  with  the  two  arms  having  one  point  in  common.  Figure  10  shows  the 
coordinate  plot,  the  plane  transform,  the  points  in  the  longest  line  found,  and  the 
estimated  line. 

This  was  a  simple  academic  configuration  which  was  intended  to  exercise  the 
code,  and  which  strives  only  to  identify  the  longer  of  the  two  lines.  By  design,  the  slopes 
of  the  two  lines  are  distinctly  different,  thus  the  plane  transform  clearly  shows  two 
distinct  intersection  regions  within  the  grid,  one  with  four  curves  and  the  other  with 
seven.  The  estimated  line  was  correctly  found  to  be  the  longer  of  the  two  lines.  Note 
that,  for  this  data  set,  the  curve  intersections  occur  at  exact  points,  and  the  estimated  line 
is,  in  fact,  the  exact  line  which  is  the  long  arm  of  the  cross. 

An  examination  of  grid  resolution  offers  some  limited  insight  for  this  set  of  data. 
In  Table  I,  results  are  tabulated  using  various  grid  sizes  for  the  number  of  bins  that 
indicate  the  possibility  of  a  line. 
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Figure  10.  (a)  Points  in  the  data  set  cross.data,  (b)  plane  transform 
of  the  points,  (c)  longest  line  of  points  found,  (d)  the  estimated  line. 


Table  I.  Number  of  bins  found  with  a  possible  line  in  cross.data. 


r- 
resolution 

9-resolution 

1 

2 

3 

5 

10 

15 

20 

1 

2 

2 

2 

2 

4 

2 

9 

2 

2 

1 

2 

2 

3 

2 

5 

3 

3 

2 

2 

2 

2 

2 

4 

5 

4 

3 

2 

2 

1 

2 

[3]* 

10 

6 

4 

3 

2 

1 

2 

[2] 

15 

10 

5 

4 

2 

2 

[2] 

[2] 

20 

14 

7 

6 

4 

3 

[2] 

[2] 

[•]  denotes  erroneous  lines. 
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The  threshold  for  the  minimum  number  of  points  to  be  considered  for  the  desired 
line  was  set  at  six,  since  it  was  already  known  that  the  longest  line  contains  seven  points. 
In  theory,  if  the  grid  resolution  is  chosen  correctly,  only  one  bin  should  result  in  a  line, 
since  only  one  line  with  more  than  (or  equal  to)  the  threshold  value  of  six  points  exists. 
From  the  table,  however,  it  is  apparent  that  multiple  grid  cells  have  captured  the  same  set 
of  curves  since  multiple  bins  seem  to  have  found  the  same  line.  It  can  also  be  seen  that 
more  bins  are  found  at  the  edges  of  the  table,  suggesting  that  cells  which  are  tall  and 
narrow  (small  d-resolution  and  large  r-resolution)  or  wide  and  flat  (large  d-resolution  and 
small  r-resolution)  provide  the  opportunity  for  more  than  one  cell  to  capture  the  same  set 
of  intersecting  curves.  (Note  that  this  is  visually  intuitive.)  For  the  more  even-sided  bins, 
the  algorithm  is  more  discriminating  and  only  one  or  two  bins  give  a  possible  line.  In  this 
case,  the  line  can  be  located  easily  regardless  of  the  resolution,  and  a  larger  grid  size 
would  more  efficient  since,  with  fewer  bins,  the  number  of  computations  would  be 
reduced. 

Lastly,  for  relatively  larger  grid  sizes,  erroneous  lines  may  be  found  (denoted  by 
brackets  around  the  entries  in  the  table).  For  this  data  set,  the  lines  are  not  actually 
erroneous,  but  they  include  an  eighth  point  which  does  not  belong  in  the  line.  That  is,  the 
eighth  point  is  not  perfectly  collinear  with  the  rest.  It  is  obvious  that  the  larger  grid  cell 
covers  a  larger  area  of  the  plane  and  will  therefore  capture  curves  that  pass  near  the 
intersection  point.  For  more  realistic  data  sets  where  points  are  not  perfectly  collinear, 
but  nearly  collinear,  this  is  precisely  the  idea  behind  the  selection  of  grid  resolution:  the 
size  of  the  cell  must  be  large  enough  to  avoid  redundancy  and  to  increase  computational 
efficiency,  but  not  too  large  that  an  unacceptable  number  of  stray  points  are  included  in 
the  final  line  estimation. 

2.  Corner  of  a  Room:  corner. data 

The  data  set  corner.data  contained  50  points  and  was  intended  to  represent  a 
sonar  image  of  a  room  corner  with  other  objects  in  the  immediate  vicinity.  As  would  be 
expected  of  real  data,  the  points  in  the  image  do  not  form  clean  lines.  Figure  1 1  shows 
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the  coordinate  plot,  the  plane  transform,  the  points  in  the  longest  line  found,  and  the 
estimated  line  for  a  given  resolution.  In  this  case,  6-resolution  is  5°  and  r-resolution  is  10 
inches. 


(a)  Points  in  corner.data 


(b)  Plane  transform 
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Figure  11.  (a)  Points  in  the  data  set  corner.data,  (b)  plane 
transform  of  the  points,  (c)  longest  line  of  points  found,  (d)  the 
estimated  line. 


In  the  coordinate  plot,  Figure  1 1(a),  the  longest  line  appears  to  be  the  vertical  line 
on  the  far  right  with  6~0°  and  r  =  80  inches.  That  the  points  in  the  image  do  not  form 
exact  lines  is  exhibited  in  the  plane  transform,  Figure  1 1(b),  which  shows  a  collection  of 
intersecting  regions  not  especially  distinguishable  from  one  another.  In  Figure  1 1(c),  a 
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set  of  points  has  been  found  which  includes  most,  but  not  all,  of  the  points  in  the  line  that 
was  expected,  and  Figure  1 1(d)  shows  the  line  estimated  from  those  points. 

In  this  data  set,  the  grid  resolution  becomes  more  significant  since  the  possible 
lines  consist  of  points  which  are  generally  not  perfectly  collinear,  and  the  problem 
becomes  one  of  not  only  finding  the  desired  line,  but  also  estimating  it  as  accurately  as 
possible.  The  accuracy  of  the  line  depends  on  the  points  contributing  to  the  line-fitting; 
that  is,  each  point  affects  the  slope  of  the  final  line.  Clearly,  the  grid  size  directly  affects 
which  points  are  included  in  a  particular  bin,  i.e.,  which  curves  are  included  in  a 
particular  cell.  Consequently,  grid  size  will  also  affect  the  total  number  of  points 
ultimately  assigned  to  each  bin.  For  example,  a  smaller  grid  size  is  expected  to  narrow 
the  spread  of  points  allowed  in  forming  a  line,  resulting  in  fewer  points  being  included  in 
the  line  estimation. 

In  Table  II,  the  number  of  points  in  the  longest  line  found  is  tabulated  for  various 
resolutions.  It  can  be  seen  that  when  the  resolution  window  (i.e.,  grid  size)  is  reduced,  the 
number  of  points  in  the  longest  line  found  is  also  reduced,  as  expected.  All  this  implies  is 
that,  with  the  smaller  grid  size,  the  set  of  points  being  selected  exhibits  a  tighter  degree  of 
collinearity  in  their  relative  positions.  The  estimate  resulting  from  this  is  only  an  estimate 
of  the  line  formed  by  that  specific  set  of  points,  but  it  is  not  necessarily  a  better  estimate 
of  the  line  that  is  desired. 

From  the  results  of  the  table,  excluding  the  bracketed  entries,  it  is  evident  that  the 
line  that  was  initially  anticipated  is  indeed  the  line  found  by  the  Hough  algorithm,  and  it 
is  the  line  that  is  desired  in  this  case.  However,  it  should  be  noted  that  there  is  no 
physical  line  in  existence  against  which  the  accuracy  of  the  estimate  can  be  evaluated. 
Although  the  image  implies  the  values  6  ~  0°  and  r  ~  80  inches,  the  data  does  not 
represent  a  real  image,  and  no  meaningful  computation  was  incorporated  into  the  data 
relating  to  the  distribution  of  points  around  a  line  having  these  specific  parameters.  In 
other  words,  the  line  did  not  cause  the  data.  Therefore,  while  the  correct  line  has  been 
extracted  by  the  algorithm,  there  is  no  reason  to  expect  that  the  line  estimate  having 
parameters  closest  to  these  values  is  necessarily  the  most  accurate  estimate. 
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Table  II.  Number  of  points  in  the  longest  line,  the  angle  6  in  radians,  and 
the  distance  r  in  inches  for  corner. data. 


r- 
resolution 

6-resolution 

1 

2 

3 

5 

10 

15 

20 

1 

10 

10 

10 

11 

14 

14 

[17]* 

0.033350 

0.033350 

0.033350 

0.044614 

0.073757 

0.073757 

2.233637 

80.4 

80.4 

80.4 

80.1 

80.3 

80.3 

13.4 

2 

10 

10 

10 

11 

14 

15 

[18] 

0.033350 

0.033350 

0.033350 

0.044614 

0.073757 

0.097673 

2.228296 

80.4 

80.4 

80.4 

80.1 

80.3 

79.9 

13.8 

3 

11 

12 

11 

12 

14 

{16}** 

[18] 

0.033371 

0.046329 

0.033371 

0.046329 

0.073757 

0.108628 

2.237988 

80.2 

80.2 

80.2 

80.2 

80.3 

80.4 

14.9 

5 

12 

12 

13 

14 

16 

{18} 

{19} 

0.032550 

0.032550 

0.027493 

0.057245 

0.068396 

0.122093 

0.132361 

80.9 

80.9 

81.2 

79.3 

79.8 

79.2 

79.6 

10 

16 

16 

16 

16 

{18} 

{18} 

{20} 

0.027340 

0.027340 

0.027340 

0.027340 

0.122093 

0.122093 

0.163183 

82.5 

82.5 

82.5 

82.5 

79.2 

79.2 

79.1 

15 

21 

21 

21 

21 

21 

{23} 

{23} 

0.056670 

0.056670 

0.056670 

0.056670 

0.056670 

0.112749 

0.112749 

81.4 

81.4 

81.4 

81.4 

81.4 

81.0 

81.0 

20 

16 

16 

16 

[18] 

[21] 

[23] 

[25] 

0.027340 

0.027340 

0.027340 

2.145404 

2.042113 

2.013184 

1.986654 

82.5 

82.5 

82.5 

10.1 

10.4 

10.4 

8.8 

*  [■]  denotes  erroneous  lines.      **  { ■ }  denotes  corner  effects. 

Two  interesting  consequences  of  choosing  the  grid  size  inappropriately  can  be 
observed.  First,  the  table  values  denoted  with  brackets  represent  lines  which  are 
prominently  different  from  the  rest,  as  evidenced  by  the  6  and  r  values  listed.  Figure 
12(a)  shows  a  representative  plot  of  this  line  using  the  points  found  for  9-resolution  of  5° 
and  r-resolution  of  20  inches.  It  appears  that  two  dense  clusters  of  points  can  give  rise  to 
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an  erroneous  line.  A  line  can  be  drawn  between  any  two  clusters,  and  if  the  two  clusters 
combined  contain  more  total  points  than  any  other  set  of  points  in  a  possible  line,  they 
will  incorrectly  be  chosen  as  the  'longest  line.'  This  is  an  effect  of  an  inappropriate  grid 
size  and  not  simply  an  effect  of  too  large  a  grid,  although  the  occurrences  of  this  effect 
are  localized  at  the  edges  of  the  table. 
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Figure  12.  Effect  of  (a)  Inappropriate  grid  size,  and  (b)  corner 
interference  in  corner. data. 


Secondly,  as  Figure  12(b)  shows,  the  corner  tends  to  interfere  with  the  line  finding 
and  has  the  effect  of  altering  the  slope  of  the  line  by  pulling  the  estimated  line  away  from 
the  vertical.  This  effect  is  present  in  varying  degrees  throughout  the  result  data,  but  most 
pronounced  at  a  few  of  the  larger  grid  sizes.  The  table  values  denoted  with  braces 
represent  lines  containing  several  points  that  are  included  on  the  corner  segment  which 
are  not  on  the  line  of  interest.  The  plot  in  the  figure  is  for  9-resolution  of  10°  and  r- 
resolution  of  10  inches. 

The  previous  paragraphs  have  attempted  to  distinguish  between  correctly  finding 
the  desired  line  and  accurately  estimating  it.  The  results  of  Table  II  show  that  both 
problems  are  influenced  by  the  resolution.  Too  large  of  a  window  can  result  in  a 
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completely  erroneous  line  or  the  correct  line  becoming  shifted  in  its  orientation.  Too 
small  of  a  window  can  impose  too  stringent  a  requirement  for  collinearity  which  is  not 
warranted  and  will  exclude  valid  data  points  which  are  important  to  the  accuracy  of  the 
estimate. 

3.  A  Corridor:  corridor. data 

A  possible  location  to  exploit  in  the  selection  of  an  extended  surface  are  the  walls 
of  a  corridor  or  hallway.  The  data  set  corridor.data  simulates  the  sonar  image  in  a 
hallway  assuming  the  robot  is  positioned  at  the  (0,0)  coordinate  position  (Figure  13(a)). 


100 

50 

0 


(a)  Points  in  corridor.data 


-150 

-150     -100      -50 


0  50         100       150 

x  (inches) 


150 


-150 


(b)  Plane  transform 


50  100  150 

angle  (degrees) 


100 
50 


(c)  Points  in  the  longest  line 


-150     -100      -50  0  50         100        150 

x  (inches) 


100 
50 


-100 


(d)  Estimated  line 


angle=  179.6  degrees 
r=  -30.54  inches 


No.  of  points  in  estimate  =  1 5 


-150 

-150     -100      -50 


0  50        100       150 

x  (inches) 


Figure  13.  (a)  Points  in  the  data  set  corridor.data,  (b)  plane  transform 
of  the  points,  (c)  longest  line  of  points  found,  (d)  the  estimated  line. 
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Figure  13  shows  the  coordinate  plot,  the  plane  transform,  the  points  in  the  longest 
line  found,  and  the  estimated  line  for  a  given  resolution.  In  this  case,  6-resolution  is  5° 
and  r-resolution  is  10  inches. 

Note  that  in  Figure  13(a),  the  coordinate  plot,  the  data  points  are  spread  further 
apart  at  the  ends  of  the  segments  to  depict  the  characteristic  spread  of  sonar  returns  at 
distances  farther  away  from  the  robot. 

The  plane  transform,  Figure  13(b),  shows  the  curves  converging  on  the  edges  of 
the  plane.  These  are  the  intersection  regions.  Recall  that  the  convention  adopted  limits  6 
to  the  interval  [0,180°]  while  r  can  take  on  both  positive  and  negative  values.  Therefore, 
a  line  with  6=0°  and  r=r0  can  equivalently  be  represented  with  6  -  180°  and  r  =  -r0. 
Each  of  the  two  lines  in  the  image  has  two  (6 ,  r)  representations  associated  with  it,  which 
translates  into  four  intersection  regions  in  the  plane  transform. 

The  line  found,  shown  in  Figure  13(c),  happens  to  be  the  wall  on  the  right  hand 
side,  although  the  input  data  included  an  equal  number  of  points  in  both  lines.  The 
selection  of  the  longest  line  in  this  situation  is  incidental;  when  more  than  one  bin  are 
found  to  contain  the  same  number  of  points,  the  algorithm  merely  selects  the  first  one  it 
encounters.  Table  III  displays  the  number  of  points  in  the  longest  line  estimate,  and  its  6 
and  r  parameter  values,  for  various  resolutions.  For  49  tests  (at  the  different  resolutions) 
of  the  algorithm,  the  right  wall  was  selected  25  times  and  the  left  wall  was  selected  24 
times.  Each  line  has  an  equal  chance  of  being  extracted  as  the  longest  line. 

As  discussed  above,  each  of  the  lines  in  this  data  set  can  be  described  equivalently 
by  two  (6 ,  r)  representations,  and,  by  design  (due  to  the  convention  adopted),  the 
algorithm  will  provide  the  pair  with  a  positive  value  for  9 .  This  can  be  seen  in  the  table. 

This  idealized  example  of  a  corridor  does  not  account  for  irregularities  in  a  real 
corridor  which  might  cause  one  of  the  walls  to  be  less  appropriate  to  use  as  a  reference. 
For  example,  support  columns  may  protrude  a  few  inches  from  the  wall  surface  and  affect 
the  sonar  reflections.  Or,  the  corridor  may  have  a  bend  so  that  one  wall  will  be  shorter 
and  the  other  will  end  with  a  corner,  thereby  introducing  corner  interference  effects. 
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Also,  doors  that  may  be  arbitrarily  left  open  or  closed  could  affect  the  selection  of  an 
appropriate  reference. 


Table  III.  Number  of  points  in  the  longest  line,  the  angle  6  in  radians,  and 
the  distance  r  in  inches  for  corridor. data.  (Shaded  entries  denote  left  wall, 
unshaded  entries  denote  right  wall.) 


r- 
resolution 

O-resolution 

1 

2 

3 

5 

10 

75 

20 

1 

11 

13 

14 

14 

15 

15 

14 

3.134430 

3.133807 

0.011145 

0.011145 

0.010817 

0.010817 

0.015370 

-30.5 

30.4 

29.4 

29.4 

29.5 

29.5 

29.3 

2 

12 

14 

14 

15 

15 

15 

15 

3.139746 

0.003419 

0.003419 

3.133933 

0.010817 

0.010817 

0.011884 

30.7 

-30.5 

-30.5 

-30.5 

29.5 

29.5 

-29.5 

3 

13 

14 

15 

15 

15 

15 

15 

0.013480 

0.003419 

3.133933 

3.133933 

0.010817 

0.010817 

0.011884 

5 

-29.3 

-30.5 

-30.5 

15 

-30.5 
15 

29.5 
15 

29.5 
15 

-29.5 
15 

13 

14 

0.013480 

0.003419 

3.133933 

3.133933 

0.010817 

0.010817 

0.011884 

10 

-29.3 

-30.5 

-30.5 
15 

-30.5 
15 

29.5 
15 

29.5 
15 

-29.5 
15 

13 

14 

0.013480 

0.003419 

3.133933 

3.133933 

0.010817 

0.010817 

0.011884 

-29.3 

-30.5 

-30.5 

-30.5 

29.5 

29.5 

-29.5 

15 

13 

14 

15 

15 

15 

15 

15 

0.013480 

0.003419 

3.133933 

3.133933 

0.010817 

0.010817 

0.011884 

-29.3 

-30.5 

-30.5 

-30.5 

29.5 

29.5 

-29.5 

20 

20 

20 

20 

20 

20 

20 

20 

0.000325 

0.000325 

0.000325 

0.000325 

0.000325 

0.000325 

0.000325 

-30.0 

-30.0 

-30.0 

-30.0 

-30.0 

-30.0 

-30.0 
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B.         REAL  ENVIRONMENT 

Using  the  Nomad  200  robot,  sonar  data  were  collected  in  the  two  environments 
which  were  simulated  with  synthetic  data:  a  corner  and  a  corridor.  It  was  desired  to 
observe  how  the  Hough  algorithm  would  the  perform  with  an  actual  sonar  image.  The 
data  collected  would  be  representative  of  typical  sonar  reflections  in  a  typical 
environment  and  may  or  may  not  have  characteristics  which  could  result  in  line  estimates 
such  as  those  obtained  from  applying  the  Hough  technique  to  the  synthetic  data. 

1.  Experimental  Considerations 

The  experiments  were  set  up  by  selecting  locations  within  the  laboratory  that 
provided  the  necessary  surroundings  and  situating  the  robot  accordingly.  For  practical 
reasons,  no  attempt  was  made  to  precisely  measure  the  position  and  orientation  of  the 
robot  while  it  was  gathering  data.  It  has  already  been  established  that  it  is  virtually 
impossible  to  position  the  robot  precisely,  even  by  lifting  it  into  position,  hence  the 
motivation  for  a  self-calibration  capability  (which  is  the  topic  of  this  work).  Further, 
precise  measurement  of  the  distances  and  orientations  of  walls  and  other  objects  in  the 
laboratory,  with  respect  to  the  robot,  was  deemed  to  offer  only  minor  benefit  to  the 
analysis.  Finally,  in  the  operational  implementation  of  a  self-calibration  capability,  what 
the  robot  sees  is  perhaps  more  relevant  than  what  physically  exists. 

Also,  no  attempt  was  made  to  achieve  the  exact  angular  rotations  for  gathering 
data.  The  robot's  movement  was  generally  within  one  degree  of  the  rotation  commanded 
and  was  considered  to  be  adequate  for  the  purpose,  i.e.,  to  obtain  additional  data  points  at 
angles  smaller  than  the  separation  angle  between  the  sensors. 

2.  Corner  Results 

A  sonar  map  of  a  corner  is  shown  in  Figure  14;  the  coordinate  plot,  plane 
transform,  a  "good  guess"  estimate,  and  an  "average"  estimate  are  shown  in  Figure  15. 
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Figure  14.  Sonar  map  of  a  corner  (#1). 


(a)  Points  from  corner  map  #1 


(b)  Plane  transform 
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(c)  "Good  guess"  line  estimate 
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(d)  Average  line  estimate 
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Figure  15.  (a)  Coordinate  plot,  (b)  plane  transform,  (c)  "good  guess" 
line  estimate,  and  (d)  average  line  estimate  for  a  comer  map  (#1). 


34 


It  is  noted  that  the  range  of  the  Nomad  200's  sonar  sensors  is  between  six  inches 
and  255  inches.  If  a  receiver  does  not  receive  any  echo  after  a  specified  time,  it  returns 
the  maximum  value  of  255  inches,  which,  in  most  cases,  indicates  absence  of  any  objects 
within  the  sonar  range.  These  "false"  points  are  eliminated  before  executing  hough.c. 
This  explains  the  difference  between  the  sonar  map  and  the  coordinate  plot. 

The  48  points  (three  robot  angular  positions  times  16  sonar  readings  per  position) 
collected  by  the  robot  are  plotted  in  Figure  15(a).  The  plane  transform,  Figure  15(b), 
indicates  five  intersection  regions  quite  clearly,  but  based  on  the  positions  of  two  lines  at 
0°  and  180°,  two  of  the  intersection  regions  are  redundant.  Figure  15(c)  shows  a  line  that 
the  Hough  algorithm  has  selected  for  a  reference  wall  directly  in  front  of  the  robot.  It  is 
labeled  a  "good  guess"  estimate  of  the  line  representing  the  wall,  meaning  that  this  line 
looks  like  a  good  estimate  of  the  correct  line,  if  this  line  were  to  be  estimated  based  on  a 
general  visual  inspection  of  the  sonar  picture.  Figure  15(d)  is  an  approximate  average 
line  estimate,  which  will  be  explained  later. 

Table  IV  gives  the  results  of  varying  d-resolution  and  r-resolution  for  the  set  of 
data  collected  for  this  corner  map.  In  all  cases  the  same  wall  was  found  as  the  best  line, 
but  the  orientation  of  that  wall  (the  angle  of  the  line's  normal  with  respect  to  the  robot), 
varies  throughout  the  table,  although,  as  with  the  synthetic  data,  the  same  line  may  be 
found  for  several  groups  of  0-resolutionl  r-resolution  combinations. 

From  the  data  given  in  the  table,  there  is  no  appreciable  trend  that  can  be 
exploited  for  perhaps  pre-selecting  an  optimum  resolution.  Using  the  angle  6  as  the 
discriminating  parameter,  since  small  variations  in  9  will  have  significant  consequences 
when  using  a  particular  line  as  a  reference,  it  is  noted  that  6  ranges  from  a  minimum 
value  of  0.0156  radians  (0.893°)  to  a  maximum  value  of  0.1609  radians  (9.22°),  and  has  a 
mean  value  of  0.0777  radians  (4.45°)  and  a  median  value  of  0.0675  radians  (3.88°). 

The  "good  guess"  estimate  depicted  in  Figure  15(c)  is  the  line  obtained  with  the 
discretization  6-resolution  =  5°  and  r-resolution  =  10  inches  (see  the  table)  and  was 
chosen  based  on  its  appearance.  The  average  line  estimate  of  Figure  15(d)  is  a  line 
obtained  with  0-resolution  =  10°  and  r-resolution  =  10  inches;  for  this  line,  0  =  0.0762 
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radians  (4.36°),  which  is  close  to  the  mean  value.  (Note:  a  line  representing  the  exact 
mean  does  not  actually  exist  in  the  experimental  results.)  The  figures  suggest  that  the 
"good  guess"  estimate  may  be  a  more  correct  representation  of  the  line;  the  average 
estimate  is  slightly  influenced  by  a  few  distant  points  which  tend  to  skew  the  line. 


Table  IV.  Number  of  points  in  the  longest  line,  the  angle  8  in  radians,  and 
the  distance  r  in  inches  for  a  corner  map  (#1). 


r- 
resolution 

6-resolution 

1 

2 

3 

5 

10 

75 

20 

1 

10 

11 

12 

15 

17 

18 

18 

0.015591 

0.020558 

0.025427 

0.067529 

0.100117 

0.147687 

0.147687 

34.6 

3407 

34.8 

34.4 

34.3 

34.1 

34.1 

2 

13 

13 

13 

15 

17 

18 

18 

0.027911 

0.027911 

0.027911 

0.067529 

0.100117 

0.147687 

0.147687 

34.8 

34.8 

34.8 

34.4 

34.3 

34.1 

34.1 

3 

13 

13 

15 

15 

17 

18 

18 

0.027911 

0.027911 

0.071760 

0.067529 

0.100117 

0.147687 

0.147687 

34.8 

34.8 

34.4 

34.4 

34.3 

34.1 

34.1 

5 

12 

13 

15 

16 

18 

19 

19 

0.029225 

0.026275 

0.049590 

0.051560 

0.086142 

0.134223 

0.134223 

33.8 

33.9 

3308 

33.9 

33.8 

33.4 

33.4 

10 

17 

17 

18 

18 

19 

20 

20 

0.019783 

0.019783 

0.041350 

0.041350 

0.076234 

0.126041 

0.126041 

34.6 

34.6 

34.4 

34.4 

34.1 

33.8 

33.8 

15 

18 

18 

18 

18 

19 

20 

20 

0.041350 

0.041350 

0.041350 

0.041350 

0.076234 

0.126041 

0.126041 

34.4 

34.4 

34.4 

34.4 

34.1 

33.8 

33.8 

20 

20 

20 

20 

20 

21 

21 

22 

0.054831 

0.054831 

0.054831 

0.054831 

0.106625 

0.106625 

0.160871 

33.2 

33.2 

33.2 

33.2 

32.7 

32.7 

32.0 

The  line  estimates  with  the  minimum  and  maximum  values  for  6  are  shown  in 
Figure   16(a)  and  (b),  respectively.  Coincidentally,  they  occur  at  the  smallest  and  the 
largest  resolution  windows,  corresponding  to  the  highest  and  the  lowest  degrees  of 
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collinearity.  The  minimum-^  estimate  includes  only  ten  points  and  represents  only  a 
short  segment  of  the  line.  The  maximum-  0  estimate  is  clearly  corrupted  by  the  distant 
points. 


(a)  Min.  theta  line  estimate 
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(b)  Max.  theta  line  estimate 
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Figure  16.  (a)  Minimum-0  line  estimate  and  (b)  maximum-0 
line  estimate  for  the  data  collected  from  corner  map  (#1). 


An  interesting  observation  that  can  be  made  from  Figure  15(c)  and  (d)  and  Figure 
16(a)  and  (b)  is  that  neither  the  corner  effects  nor  the  clustering  effects  were  seen  in  this 
data. 

A  second  set  of  sonar  data  was  obtained  for  the  corner  configuration  with  the 
robot  located  so  that  it  was  at  almost  equal  distance  from  both  walls  of  the  corner.  The 
sonar  map  is  shown  in  Figure  17.  The  coordinate  plot,  plane  transform,  a  "good  guess" 
line  estimate  and  an  average  line  estimate  are  shown  in  Figure  18.  Results  are  shown  in 
Table  V.  The  "good  guess"  estimate  is  the  line  with  6-resolution  =  5°  and  r-resolution  = 
10  inches.  In  this  data  set,  6  ranges  from  a  minimum  value  of  0.1218  radians  (6.98°)  to  a 
maximum  value  of  0.2972  radians  (17.03°),  and  has  a  mean  value  of  0.2090  radians 
(1 1.97°)  and  a  median  value  of  0.1975  radians  (1 1.32°).  The  average  line  estimate  of 
Figure  18(d)  is  a  line  obtained  with  0-resolution  =  15°  and  r-resolution  =  10  inches;  for 
this  line,  0=  0.1975  radians,  which  is  the  closest  line  to  the  mean  in  the  table  and  is  also 
the  median  value.  In  this  case,  the  difference  between  the  "good  guess"  and  the  average 
estimates  is  only  slightly  discernible  from  the  plots. 
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Figure  17.  Sonar  map  of  a  comer  (#2). 
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(b)  Plane  transform 
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(c)  "Good  guess"  line  estimate 


(d)  Average  line  estimate 
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Figure  18.  (a)  Coordinate  plot,  (b)  plane  transform,  (c)  "good  guess" 
line  estimate,  and  (d)  average  line  estimate  for  a  corner  map  (#2). 
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Table  V.  Number  of  points  in  the  longest  line,  the  angle  6  in  radians,  and  the 
distance  r  in  inches  for  corner  map  (#2). 


r- 
resolution 

^-resolution 

1 

2 

3 

5 

10 

15 

20 

1 

12 

12 

13 

13 

16 

17 

20 

0.184700 

0.184700 

0.172176 

0.189081 

0.297184 

0.126183 

0.250375 

33.6 

33.6 

33.7 

33.7 

33.3 

32.9 

32.0 

2 

11 

12 

13 

14 

16 

18 

20 

0.182979 

0.184700 

0.172176 

0.231639 

0.297184 

0.160909 

0.250375 

33.6 

33.6 

33.7 

33.3 

33.3 

32.4 

32.0 

3 

14 

15 

15 

15 

18 

17 

20 

0.170384 

0.170761 

0.170761 

0.170761 

0.284404 

0.194181 

0.250375 

34.0 

33.9 

33.9 

33.9 

34.0 

31.9 

32.0 

5 

14 

14 

15 

15 

16 

19 

20 

0.167143 

0.140667 

0.170761 

0.148742 

0.121845 

0.197465 

0.250375 

33.8 

33.0 

33.9 

33.2 

32.6 

32.1 

32.0 

10 

16 

16 

17 

17 

18 

19 

20 

0.153219 

0.153219 

0.236717 

0.236717 

0.284404 

0.197465 

0.250375 

33.4 

33.4 

33.8 

33.8 

34.0 

32.1 

32.0 

15 

17 

17 

17 

18 

18 

19 

20 

0.236717 

0.236717 

0.236717 

0.284404 

0.284404 

0.197465 

0.250375 

33.8 

33.8 

33.8 

34.0 

34.0 

32.1 

32.0 

20 

20 

20 

21 

21 

21 

21 

21 

0.183997 

0.183997 

0.238805 

0.238805 

0.238805 

0.238805 

0.238805 

31.5 

31.5 

31.2 

31.2 

31.2 

31.2 

31.2 

Figure  19(a)  and  (b)  are  the  minimum-0  and  maximum-0  line  estimates.  Here, 
these  6  values  occur  at  6-resolution  =  10°  and  r-resolution  =  5  inches  for  the  minimum 
and  6-resolution  =  10°  and  r-resolution  =  2  inches  for  the  maximum.  In  Figure  19(a), 
there  is  a  slight  corner  effect.  In  this  data  set,  the  corner  effect  is  evidenced  by  smaller  Q 
values,  and  since  this  is  the  minimum-  0  line,  this  is  the  line  with  the  strongest  corner 
effect  that  was  found.  As  in  the  first  corner  map  results,  the  maximum- 0  line  again 
shows  the  effect  of  distant  points,  and  the  line  is  skewed. 
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(a)  Min.  theta  line  estimate 
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Figure  19.  (a)  Minimum-0  line  estimate  and  (b)  maximum-0 
line  estimate  for  the  data  collected  from  corner  map  (#2). 


3. 


Corridor  Results 


The  sonar  map  for  the  data  collected  in  a  corridor  is  shown  in  Figure  20.  The 
coordinate  plot,  plane  transform,  and  line  estimates  of  each  of  the  two  possible  lines  are 
shown  in  Figure  2 1 .  In  this  scenario,  the  two  walls  of  the  corridor  were  not  equivalent  in 
that  the  wall  to  the  left  of  the  robot  (in  the  top  half  of  the  plot)  was  not  a  smooth  extended 
surface  and  was  cluttered  by  various  objects,  e.g.,  chalk  board,  doors,  a  support  column, 
and  a  fire  extinguisher.  The  correct  wall  would  then  be  the  wall  to  the  right  of  the  robot, 
depicted  in  Figure  21(c).  The  other  wall  is  shown  in  Figure  21(d). 

The  data  set  was  subjected  to  the  same  resolution  variation  exercise  as  was  done 
for  the  corner  data.  The  results  are  tabulated  in  Table  VI.  In  general,  the  algorithm 
successfully  found  the  correct  line;  the  cases  where  the  'wrong'  line  was  found  are  shaded 
in  the  table.  For  the  correct  lines  in  the  data  set,  6  ranges  from  a  minimum  of  1.6123 
radians  (92.38°)  to  a  maximum  of  1.7159  radians  (98.31°),  and  has  a  mean  of  1.6508 
radians  (94.58°)  and  a  median  of  1.6468  radians  (94.35°). 
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Figure  20.  Sonar  map  of  a  corridor. 
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(b)  Plane  transform 
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(d)  Wrong  line  estimate 
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Figure  21.  (a)  Coordinate  plot,  (b)  plane  transform,  (c)  correct 
line  estimate,  and  (d)  wrong  line  estimate  for  corridor  map. 
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Table  VI.  Number  of  points  in  the  longest  line,  the  angle  6  in  radians,  and 
the  distance  r  in  inches  for  the  corridor  map.  (Shading  denotes  wrong  line 
found.) 


r- 
resolution 

0-resolution 

1 

2 

3 

5 

10 

15 

20 

1 

11 

14 

14 

13 

18 

20 

19 

1 .649920 

1.656177 

1.656177 

1.627800 

1.640387 

1.700963 

1.619910 

-41.4 

-41.4 

-41.4 

-41.0 

-40.7 

-39.6 

-40.3 

2 

14 

16 

17 

16 

18 

21 

19 

1.642547 

1.661432 

1.653174 

1.630646 

1.640387 

1.715913 

1.619910 

-41.0 

-41.2 

-41.0 

-40.7 

-40.7 

-39.0 

-40.3 

3 

15 

16 

17 

18 

19 

22 

20 

1.643732 

1 .630646 

1.653174 

1.615943 

1.619910 

1.689885 

1 .602665 

-41.0 

-40.7 

-41.0 

-40.0 

-40.3 

-38.1 

40.8 

5 

17 

17 

18 

18 

19 

21 

21 

1.662240 

1.662240 

1.653722 

1.653118 

1.640618 

1.715913 

1.606840 

-41.3 

-41.3 

-41.1 

-41.0 

-40.8 

-39.0 

40.4 

10 

17 

18 

18 

19 

20 

21 

22 

1.662240 

1.701811 

1.653722 

1.705929 

1.670125 

1.715913 

1.612268 

15 

-41.3 

20 

44.1 
20 

-41.1 

21 

43.9 

42.9 

-39.0 
23 

41.0 

23 

21 

22 

1.619825 

1.619825 

1.628448 

1.628448 

1.656345 

1.692189 

1.621339 

20 

-40.4 

-40.4 

-39.8 
18 

-39.8 

-38.8 

-37.9 

21 

-37.5 
23 

17 

18 

19 

20 

1.615939 

1.701811 

1.653722 

1 .705929 

1.670125 

1.715913 

1.614380 

45.0 

44.1 

-41.1 

43.9 

42.9 

-39.0 

-34.0 

Several  of  the  cases  of  a  'wrong'  line  occur  with  r-resolution  of  10  inches  and  20 
inches.    Consider  that  a  grid  which  is  row-discretized  using  a  10-inch  row  increment  will 
have  every  other  row  division  coinciding  with  the  row  divisions  of  a  grid  using  a  20-inch 
row  increment.  Therefore,  if  0-resolution  is  the  same  for  both  grids,  each  cell  formed  in 
the  20-inch  grid  will  exactly  overlap  two  cells  in  the  10-inch  grid.  It  is  reasonable  to 
expect  that  for  every  cell  in  the  10-inch  grid,  there  is  a  corresponding  cell  in  the  20-inch 
grid  that  will  contain  at  least  all  the  same  curves.  Therefore,  for  the  10-inch  grid  cell  that 
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corresponds  to  the  accumulator  bin  with  the  most  points,  if  the  corresponding  20-inch 
grid  cell  does  not  include  any  other  curve,  then  both  grids  will  produce  the  same  'longest' 
line.  In  the  table,  there  are  three  instances  of  the  same  wrong  line  resulting  from  both  the 
10-inch  and  the  20-inch  row  discretizations,  and  five  instances  of  the  same  longest  line 
(regardless  of  whether  or  not  it  is  a  correct  line). 

In  this  real  corridor  scenario,  where  one  wall  was  in  fact  preferable  to  the  other, 
the  algorithm  was  able  to  select  the  preferred  (correct)  wall  in  40  of  the  49  resolution 
windows  tested,  while  with  the  synthetic  data,  where  neither  wall  was  considered  to  be 
preferred,  each  wall  was  selected  for  about  half  the  resolution  windows  tested.  This  is 
perhaps  indicative  of  the  fact  that,  with  real  data,  noisy  surfaces  will  generate  noisy  data 
due  not  only  to  the  actual  surface  being  uneven,  but  also  because  the  sonar  returns  will  be 
reflected  unevenly. 
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V.  DISCUSSION 

In  implementing  the  Hough  technique  for  finding  straight  lines  in  a  sonar  image, 
this  study  has  shown  that  a  mobile  robot  equipped  with  sonar  sensors  can  automatically 
find  and  select  a  wall  in  its  environment  as  a  reference  for  calibrating  its  position  and 
orientation.  Some  issues  regarding  the  selection  and  estimation  of  the  extracted  line  must 
be  discussed. 

The  feature  used  to  select  the  reference  was  the  longest  line  found  in  the  image.  A 
concern  in  selecting  the  longest  line  is  that  the  relative  extent  of  walls  and  wall-like 
obstacles  is  represented  reasonably  well.  The  rationale  was  applied  (in  Chapter  III, 
Section  B)  that  a  line  found  to  have  the  most  points  would  likely  be  the  longest  line  and, 
therefore,  would  represent  an  appropriate  reference  wall.  This  is  a  relatively  safe 
assumption  as  long  as  the  data  points  are  fairly  evenly  distributed  in  the  image  obtained. 
For  the  data  points  to  be  evenly  distributed,  the  data  collection  procedure  must  be 
configured  properly.  Also,  the  obstacles  in  the  local  area  must  be  at  comparable  distances 
so  that  there  is  little  disparity  in  the  density  of  the  data  points  representing  potential 
reference  walls.  For  this  study,  the  data  points,  i.e.,  sonar  reflection  points,  were 
distributed  evenly  so  that  a  short  line  segment  was  not  likely  to  have  a  large  number  of 
densely-spaced  points  in  it.  Therefore,  the  line  with  the  most  points  was  likely  to  be  the 
longest  line. 

A  second  concern  is  in  selecting  the  correct  line.  Both  the  synthetic  and  real  data 
demonstrated  the  occurrence  of  erroneous  lines.  In  the  synthetic  corner  data,  it  was  seen 
that  dense  clusters  of  points  could  lead  to  a  longest  line  selection  that  was  entirely 
unsuitable.  That  is,  a  line  formed  by  a  pair  of  dense  clusters  was  selected  as  the  reference 
wall  when,  in  actuality,  a  wall  did  not  exist  there.  It  was  noted,  however,  that  the  real 
corner  data  exhibited  smoother  and  more  even  traces  of  the  image  than  the  synthetic 
corner  data  (Figure  1 1(a)  versus  Figures  15(a)  and  18(a)),  and  the  possibility  of  clustering 
was  not  as  great.  As  a  result,  the  correct  (desired)  line  was  likely  to  be  selected  with  the 
real  data.  The  more  difficult  problem  was  the  real  corridor  scenario  where  one  wall  was 
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preferable  to  the  other.  From  the  robot's  point  of  view,  the  two  sides  of  a  corridor  could 
look  equally  acceptable  while,  in  reality,  one  side  may  be  cluttered  or  non-contiguous. 

Given  that  the  selected  line  is  the  desired  'correct'  line,  an  attempt  was  made  to 
relate  the  resolution  window  to  the  accuracy  of  the  selected  line.  The  accuracy  depends, 
of  course,  on  the  points  used  in  the  line  fit  which,  in  turn,  are  influenced  by  the  resolution 
window.  It  seems  that  if  the  resolution  window  could  be  chosen  such  that  it  correlated  to 
the  allowed  tolerance  in  collinearity,  then  the  line  fit  would  produce  an  accurate  line. 
Several  factors  must  be  considered  in  determining  the  tolerance  for  collinearity.  The 
robot's  measurement  precision  will  affect  the  variance  in  the  data  provided  by  the  robot 
as  points  where  an  obstacle  has  been  encountered.  Likewise,  the  physical  nature  of  the 
sonar  reflections  will  also  affect  the  variance  in  the  data  since  the  echoes  are  affected  by 
the  type  and  angle  of  the  surface  of  reflection.  This  variance  in  the  data  determines  the 
tolerance  for  collinearity.  A  proper  choice  for  the  resolution  window  which  is  based  on 
the  variance  in  the  data  may  produce  an  accurate  estimate  of  that  line. 

This  study  did  not  address  line  accuracy.  This  was  an  acceptable  omission  since 
the  main  purpose  of  this  work  was  to  demonstrate  the  viability  of  the  Hough  technique  in 
enabling  a  robot  to  calibrate  itself  automatically,  and  not  necessarily  to  verify  the 
parameters  of  the  selected  line.  It  was  stated  in  Chapter  IV,  Section  B,  Sub-section  1  that 
the  Nomad  200' s  exact  position  and  orientation  were  not  measured  precisely,  and  neither 
were  the  exact  distances  and  orientations  of  the  obstacles  around  it,  so  there  was  no  way 
of  determining  the  accuracy  of  the  estimate.  Instead,  the  lines  resulting  from  varying  the 
resolution  window  for  each  set  of  data  points  were  compared  against  each  other  to 
observe  the  influence  of  the  resolution  window.    However,  in  order  to  establish  an 
optimum  resolution  window,  line  accuracy  must  be  studied  further  and  precise 
measurements  in  both  the  world  coordinate  system  and  the  robot  coordinate  system  must 
be  made.  If  the  accuracy  of  the  estimated  line  can  be  controlled,  the  corner  effects  or  the 
effects  of  stray  points  at  either  end  of  the  line,  both  of  which  change  the  orientation  of  the 
line,  may  be  eliminated. 


46 


VI.  CONCLUDING  REMARKS  AND  FUTURE  WORK 

The  calibration  of  mobile  robot  coordinates  in  a  laboratory  environment  has  been 
investigated,  and  algorithms  have  been  developed  that  allow  a  mobile  robot  to 
automatically  select  existing  wall-like  features  in  the  laboratory  as  references  against 
which  to  localize  itself.  The  Hough  technique  for  finding  straight  lines  was  shown  to  be  a 
viable  method  for  processing  a  sonar  image  obtained  by  the  Nomad  200  robot.  The  sonar 
data  collected  by  the  Nomad  200  resulted  in  plane  transforms  with  quite  distinct 
intersection  regions  which  enabled  the  implementation  of  the  Hough  algorithm. 
Additionally,  the  capability  to  automatically  and  simultaneously  select  and  recognize  a 
feature  (the  longest  wall)  for  calibration,  made  possible  by  using  the  Hough  transform, 
demonstrated  the  effectiveness  of  the  proposed  localization  method. 

Selecting  the  reference  based  on  a  feature  such  as  the  longest  line  in  the  image 
was  a  straightforward  approach.  The  longest  line  assumption  provided  a  practical 
selection  criterion  which  enabled  the  method  to  find  the  desired  line  in  all  cases  using  the 
real  sonar  data. 

The  influence  of  the  resolution  window  was  examined  to  observe  some 
generalities  in  the  line  extracted  from  the  image.  As  expected,  reducing  the  window 
provided  a  good  line  fit,  but  it  was  noted  that  a  good  line  fit  does  not  necessarily  result  in 
the  line  which  represents  the  reference  wall  most  accurately.  The  accuracy  of  the  line 
estimation  can  only  be  determined  if  the  precise  starting  positions  and  orientations  of  the 
robot  and  the  obstacles  (potential  reference  features)  around  it  are  known.  In  future  work, 
a  statistical  study  of  the  sonar  reflections,  along  with  precise  measurements  of  positions 
and  orientations  of  obstacles  in  relation  to  the  robot's  position  and  orientation,  is 
recommended  so  that  further  study  can  be  pursued  to  optimize  the  resolution  window. 

Also  for  future  work,  the  algorithm  can  be  augmented  with  a  capability  to  find  a 
second  reference  wall  which  is  perpendicular  to  the  first.  Hence,  both  of  the  positional 
coordinates,  x  and  y,  of  the  mobile  robot  can  be  calibrated;  that  is  the  robot  can  be 
calibrated  in  two  dimensions. 
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APPENDIX  A.  HOUGH  ALGORITHM  (hough.c) 

/**********+**********+**********************+*»********, ^i^^^^^^^^^^^^^^^^^^^^^^.^^^^^^.^ 

*  PROGRAM:  hough.c 

■*■ 

*  PURPOSE:  To  implement  the  Hough  Transform  algorithm  given  a  set  of  coordinate  data 

*  points. 


r*************^ 


****************************************************  ******»**.,.***.*.** 

#include  <stdio.h> 
#include  <stdlib.h> 

iinclude  <math.h> 

#define  THETA_RESOLUTION  10.0 
ttdefine  R_RESOLUTION  10.0 
fdefine  PI  3.1415926 

struct  point  { 

int  data_index; 

struct  point  *prev_point; 
}; 

struct  accumulator  { 

int  pt_counter; 

struct  point  *last_point; 
}; 

void  hough_trans form ( float  *all_data,  mt  *no_of_pts,  int  *line_data_index,  int 

*no_of_pts_on_line) 

{ 

int  i, j , k,m; 

int  no_of_all_data; 

int  M_col,  N_row; 

float  x,  y,  max_x,  max_y; 

float  theta,  r,  nxt_theta,  nxt_r,  r_max,  R,  R_max; 

int  row,  nxt_row,  low_row,  high_row; 

struct  accumulator  *A; 

struct  point  *current_point,  *temp_point; 

int  THRESHOLD,  No_of_lines,  most_points ; 

struct  accumulator  *max_bin; 

int  *pt_list; 

no_of_all_data  =  *no_of_pts; 

printf ("\nTHETA  RESOLUTION=  %f  \nR  RESOLUTION=  %f  \n" , THETA_RESOLUTION,  R_RESOLUTION) 

/*  Determine  the  maximum  R  possible.   Read  the  input  x  and  y  coordinates,  determine 
the  distance  from  the  origin  of  each  point,  and  find  the  maximum  distance  so  that 
the  range  of  r  can  be  bounded.*/ 

R_max=  0.0;  /*Initialize  R_max*/ 

for  (i=0;  i<no_of_all_data;  i++)      /*Read  in  points  and  determine  distance*/ 
{ 

x=  * (all_data+2*i) ; 
y=  * (all_data+2*i+l) ; 
R=  sqrt(x*x  +  y*y) ; 

R_max=  (R_max  >  R)  ?  R_max  :  R;      /*Find  the  maximum  value  for  R*/ 
} 

/*Eind  maximum  r  divisible  by  R_RESOLUTION  so  we  can  discretize  later  to  rows*/ 
r  max=  R  RESOLUTION*ceil (R  max/R  RESOLUTION); 
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printf ("R_max=%f,   r_max=%f  \n",  R_max,  r_max); 

/*  Define  the  accumulator  array.   We  determine  the  number  of  columns,  M  col,  of 
discrete  theta  increments  and  the  number  of  rows,  N_row,  of  discrete  r_ increments 
which  make  up  the  accumulator  array.   Each  bin  A  is  indexed  by  (row, column) . */ 

M_col=  (int)  (180.0/THETA_RESOLUTION) ;         /-Total  number  of  columns*/ 
N_row=  (int)  (2 . 0* (r_max/R_RESOLUTION) ) ;       /*Total  number  of  rows*/ 

printf ( "Accumulator  array:   %d  columns,  %d  rows  \n",  M_col,  N_row) ; 
printf ("Accumulator  %d  %d\n" , sizeof (struct  accumulator) , sizeof (struct 
accumulator) *M  col*N  row); 


/*Allocate  memory  for  accumulator*/ 
A  =  (struct  accumulator  * ) calloc (M_col*N_row,  sizeof (struct  accumulator)); 
printf ("Accumulator  memory  allocated. \n") ; 

/-Initialize  the  accumulator  bins  and  point  pointer*/ 
for  (i=0;  i<M_col*N_row;  i++) 
{ 
(A+i) ->pt_counter=  0;  /*Counter  starts  at  zero*/ 

(A+i) ->last_point=  NULL;  /*Point  to  nothing  (NULL)*/ 

} 
printf ("Accumulator  bins  and  point  pointer  initialized. \n" )  ; 

/*  Evaluate  each  point  in  the  parameter  domain.   We  generate  a  theta-r  curve 
corresponding  to  each  point  and  mark  the  bins  that  it  crosses.   Points  that  fall 
in  the  same  bin  will  lie  approximately  in  a  line  in  the  x-y  plane.   As  we  keep 
count  of  the  number  of  curves  crossing  each  bin,  we  also  store  the  indices  of  the 
corresponding  points  and  the  address  of  the  last  point  that  was  stored  so  that 
we  can  extract  the  coordinates  later.*/ 

for  (k=0;  k<no_of_all_data;  k++) 
{ 

x=* (all_data+2*k) ;  /-Read  the  x-coordinate*/ 

y=* (all_data+2*k+l) ;  /*Read  the  y-coordinate*/ 

/*Look  at  the  accumulator  array  column  by  column.   In  each  column,  determine  the 
row(s)  that  the  curve  crosses.   This  gives  us  the  individual  bins  that  the 
curve  crosses.*/ 

for  (j=0;  j<M_col;  j++)         /*Generate  theta-r  curve,  j  is  the  column  number*/ 
{ 
theta=  (float)  j  *  THETA_RESOLUTION;  /*First  value  of  theta  to  check*/ 

r=  x*cos (theta*PI/180.0)  +  y*sin (theta*PI/180 . 0) ; 
row=  (int)  ( (r+r_max) /R_RESOLUTION)  ;  /*Row  below  first  r*/ 

nxt_theta=  ( float ) (j+1) *THETA_RESOLUTION;       /*Next  value  of  theta  to  check*/ 

nxt_r=  x*cos (nxt_theta*PI/180.0)  +  y*sin (nxt_theta*PI/180 . 0) ; 

nxt_row=  (int) ( (nxt_r+r_max) /R_RESOLUTION) ;     /*Row  below  the  second  r*/ 

low_row  =  (row  <  nxt_row)  ?  row  :  nxt_row;    /*Lowest  row  that  curve  crosses*/ 
high_row  =  (row  >  nxt_row)  ?  row  :  nxt_row;   /*Highest  row  that  curve  crosses*/ 

/*The  following  three-step  procedure  marks  the  appropriate  bins  in  the  column*/ 

for  (i=low_row;  i<=high_row;  i++)  /*Row  #s  of  bins  to  increment*/ 

{  /*i  is  the  row  number  of  the  bins*/ 

/*(1)  Increment  the  number  of  points*/ 

(A+i*M_col+ j ) ->  pt_counter  +=1; 

/*(2)  Create  a  new  point  structure  that  will  contain  the  current  point  index 
and  point  to  the  point  structure  containing  the  previous  point*/ 

if  ( ! (current_point  =  (struct  point  *)  calloc ( 1, sizeof (struct  point)))) 
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{ 

printfC'No  heap  memory  \n"); 
exit  (1)  ; 
} 

/*(3)  Assign  values  to  store  the  current  point  and  point  to  the  previous  one*/ 

current_point->data_index=  k; 

current_point->prev_point=  (A+i*M_col+] ) ->  last_point; 

(A+i*M_col+j ) ->last_point=  current_point ;        /*Keep  new  point  in  the  bin*/ 

/*printf ( "Point  index=%d  col=%d  row=%d   counter=%d  \n",  k,  j,  i, (A+i*M_col+ j ) - 
>pt_counter) ;  */ 
} 
} 
} 

/*  Determine  the  number  of  possible  lines  that  are  present  in  the  data.   This  is 
found  by  examining  the  total  number  of  points  in  each  accumulator  bin  and  comparing 
it  to  a  pre-selected  threshold  for  the  number  of  points  we  will  consider  to  be 
adequate  to  mark  a  line.   We  also  now  call  back  the  point  indices  of  the  curves 
contributing  to  each  bin  total.  */ 

THRESHOLD=  15;  /*Threshold  number  (integer)  of  points  for  a  line*/ 

No_of_lines=  0;  /'Initialize  the  line  counter*/ 

most_points=  0;  /*Initialize  the  counter  to  find  the  most  points  in  a  bin*/ 

printf ("\nThreshold=  %d  points  \n",  THRESHOLD); 

for  (j=0;  j<M_col;  j++) 
{ 
for  (i=0;  i<N_row;  i++) 
{ 
if  ( (A+i*M_col+j) ->pt_counter  >=  THRESHOLD) 
{ 

printf ("  Col  %d   Row  %d,  no.  of  points  in  bin=%d,  points:",  j,  i, (A+i*M_col+ j ) 
>pt_counter) ; 

/*To  determine  which  points  are  on  this  line — */ 

current_point=  (A+i*M_col+j ) ->  last_point; 
for  (m=0;  m< (A+i*M_col+ j ) ->  pt_counter;  m++) 

{ 

printf ("  %d  ",  current_point->data_index) ; 

current_point=  cur rent_point->prev_point ; 
} 

printf  ("\n") ; 
No  of  lines  +=1  ;  /*Cumulative  count  of  lines  in  each  bin*/ 

/♦Determine  bin  with  the  most  points*/ 

if  (most_points  <  (A+i*M_col+j ) ->pt_counter ) 
{ 

most_points=  (A+i*M_col  +  j ) ->pt_counter  ; 
max_bin=  (A+i*M_col+j ) ; 
} 
} 
} 
} 

if  (most_points  ==  0) { 

printf ("THRESHOLD  MAY  BE  TOO  HIGH... CHECK  AND  RESET  \n"); 
exit  (1) ; 
} 

printf ("Total  number  of  lines  found=  %d  \n",  No_of_lines )  ; 
printf ("Most  points  found  in  one  bin=  %d  \n",  most_points) ; 

current_point=max_bin->last_point; 

pt  list=  malloc (most_points*sizeof (int)  )  ; 
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m=0; 

while  (current_point) { 

pt_list [m++] =  current_point->data_index; 

current_point=currer.t_point->prev_point; 
} 

/*   printf ("Point  list  for  the  bin  with  the  most  points:  \n' 

for  (m=0;  m<most_points ;  m++) 
printf ("%d  \n",  pt_list [m] ) ; 
*/ 

*no_of_pts_on_line  =  most_points; 

for  (m=0;  m<most_points ;  m++) 

line  data_index [m]  =  pt_list [m] ; 


/*Free  up  the  heap  memory  that  was  being  used  for  the  chains  of  point  structures 
and  for  the  accumulator  array.*/ 

for  (j=0;  j<M_col;  j++) 
for  (i=0;  i<N_row;  i++) { 

current_point= (A+i*M_col+j ) ->last_point; 
while  (current_point) { 

temp_point=current_point->prev_point; 
cfree (current_point ) ; 
cur rent_point=temp_point ; 
} 
} 
cfree (A) ; 

/♦Should  add  code  to  move  the  point  list  to  the  place  in  memory  where  the 
accumulator  array  started,  i.e.,  immediately  after  all_data.*/ 

printf ( "\nEnd  of  hough. c  \n"); 

}  /*THIS  IS  THE  LAST  BRACKET*/ 


52 


APPENDIX  B.  LINE  ESTIMATION  ALGORITHM  {linest.c) 

/*±±  +  ***  +  *  +  +  *******  +  ±****  +  +  ir  +  +  +  +  ic  +  *±±±  +  +  *  +  ±  +  +  +  +  **  +  +  +  +  +  +  +  +  +  +  +  +  +  +  ±  +  +  +  +  +  +  +  +  i.ir  +  irir  +  +  ir 

*  PROGRAM:     linest.c 
* 

*  PURPOSE:  To  estimate  a  line  in  the  normal  parameters  given  a  set  of  points 

*  extracted  by  the  program  hough. c. 

#include  <stdio.h> 
#include  <stdlib.h> 
#include  <math.h> 

#define  PI  3.1415926 

void  line_estimation (float  *all_data,  int  *line_data_index,  int 
*no_of_pts_on_line) 

{ 

int  i, j , k,m; 

int  no_of_line_data; 

double  x,  x_2,  y,  y_2,  xy; 

double  sum_x,  sum_y,  sum_x_2,  sum  y  2,  sum  xy; 

double  mul,  mu2,  theta,  r; 

double  x_intercept,  y_intercept; 

no_of_line_data  =  *no_of_pts_on_line; 

printf ( "\nLine  estimation  using  %d  points  \n",  no_of_line_data) ; 

/* Determine  r  and  theta */ 

sum_x=  sum_y=  sum_x_2=  sum_y_2=  sum_xy=  r=  0.0; 

for  (i=0;  i<no_of_line_data;  i++)  { 
x=  * (all_data+2*line_data_index [i]  )  ; 
x_2  =  x  *  x ; 

y=   *  (all_data  +  2*line_data_index[i]+l)  ; 
y_2=  y*y; 

xy=  x*y; 
sum_x  +=  x; 
sum_y  +=  y; 
sum_x_2  +=  x_2  ; 
sum_y_2  +=  y_2 ; 
sum_xy  +=  xy; 
} 

mul=  sum_y_2  -  sum_x_2  +  ( (sum_x*sum_x) - (sum_y*sum_y) )/( (double) 
no_of_line_data) ; 

mu2=  sum_xy  -  (sum_x*sum_y) /( (double)  no_of_line_data) ; 

theta=  .5  *  atan2 ( (-2*mu2) ,mul) ; 

printf ("\n (Computed  theta=  %f)  \n", theta); 

if  (theta  <  0.0)  theta=  theta  +  PI; 

printf ( "\nCoordinates  of  points  on  the  line:\n"); 
for  (i=0;  i<no_of_line_data;  i  +  +)  { 
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x=  * (all_data+2*line_data_index [i] ) ; 
y=  * (all_data+2*line_data_index[i]+l) ; 

r  +=  ( (x*cos  (theta)  +  y*sin (theta) )/( (double)  no_of_line_data) ) ; 
printf("%f,    %f;  \n",x,y); 
} 

printf ("\nmul=  %f,  mu2=  %f,  r=  %f,  theta=  %f  \n",mul,mu2, r, theta) ; 
/*  LAST  BRACKET  */ 


54 


APPENDIX  C.  SONAR  DATA  COLLECTION  PROGRAM  (collect _data.c) 


'**********•**********  +  +  ***  +  *********  +  +  *  +  *  +  **  +  ***  +  **  +  +  +  +  •***•*  +  **+*•  +  *  +  **  +  *  +  ■*■*■** 
* 

*  PROGRAM:  collect_data . c 

*  PURPOSE:  To  collect  16*3=48  data  by  rotating  the  robot  7.5  degrees  twice. 

*  The  48  x-y  data  in  the  robot's  start-up  coordinates  are  in  all_data 

*  format,  that  can  be  passed  to  the  hough_transf orm( )  directly. 
* 

*  Add-on:  move  the  robot  in  newly  calibrated  x  coordinate. 
* 

+  *********  +  ****  +  ****  +  ****  +  *******  +  *******  +  *  +  ****************************•***■*•*  +  / 


/***  Include  Files  ***/ 

#include  "Nclient.h" 
#include  <stdio.h> 
#include  <stdlib.h> 
#include  <math.h> 

#define  ROBOT_RADIUS  8.81 
#define  PI  3.1415927 

/***  Function  Prototypes  ***/ 

void  GetSensorData (void) ; 
void  collect_xy ( ) ; 
void  hough_transform( ) ; 
void  line_estimation ( ) ; 

/***  Globals  ***/ 

long  SonarRange [16] ;   /*  array  of  sonar  readings  (inches)  */ 
long  IRRange [16] ;   /*  array  of  infrared  readings  (no  units)  */ 
long  robot_conf ig [4 ] ; 

float  *all_data; 

int  *line_data_index; 

int  no  of  line  data; 


/***  Main  Program  ***/ 

main  (unsigned  int  argc,  char**  argv) 
{ 

int  i,  j ,  index; 

int  order [16] ; 

FILE  *fp,  *fp2; 

int  max_no_of_all_data  =48;    /*    16 
int  no  of  all  data; 


/*  Connect  to  Nserver.  The  parameter  passed  must  always  be  1 .  */ 
connect  robot (1); 


/*  Initialize  Smask  and  send  to  robot.  Smask  is  a  large  array  that 
controls  which  data  the  robot  returns  back  to  the  server.  This 
function  tells  the  robot  to  give  us  everything.  */ 
init_mask ( ) ; 
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/*  Configure  timeout  (given  in  seconds) .  This  is  how  long  the  robot 
will  keep  moving  if  you  become  disconnected.  Set  this  low  if  there 
are  walls  nearby.  */ 
conf  tm(10) ; 


/*  Sonar  setup   */ 

for  (i  =  0;  i  <  16;  i++) 

order [i]  =  i; 
conf_sn (1, order) ; 

/*  Zero  the  robot.  This  aligns  the  turret  and  steering  angles.  The 
repositioning  junk  is  neccessary  to  allow  the  user  to  position 
the  robot,  though  don't  ask  me  why.  */ 
zr();   /*  tell  robot  to  zero  itself  */ 


} 


fp  =  f open ( "all. dat",  "w")  ; 
fp2  =  f open ("line. dat",  "w"); 

/*  Main   */ 


/*  allocate  memory  space  for  all_data  */ 

all_data  =  (float  *)  malloc(  sizeof (float)  *  2  *  max_no_of_all_data) 

/*  get  all_data  */ 

collect_xy (all  data,  &no_of_all_data)  ; 


/*  call  hough_transform( )  here  */ 

line_data_index  =  malloc(  sizeof (int)  *   no_of_all_data) ; 
for  (i=0;  i<no_of_all_data;  i++) 
* (line_data_index+i)  =  0; 

hough_transform(all_data,  &no_of_all_data,  line_data_index,  &no_of_line_data) 

/*  call  lineO  here  */ 

line  estimation (all  data,  line  data  index,  &no  of  line  data); 


/*  for  testing  purpose  only.   write  all_data  to  a  file.  */ 

for  (i=0;  i<no_of_all_data;  i++) 
{ 

fprintf(fp,  "  %10.2f",     * (all_data+2*i) ) ;     /*  x  coordinate  */ 
fprintf(fp,  "  %10.2f  \n",  * (all_data+2*i+l) ) ;   /*  y  coordinate  V 


for  (i=0;  i<no_of_line_data;  i++) 
{ 

fprintf (fp2, "  %f    ",  * (all_data+2*line_data_index [i] ) ) ;      /*  x-coord  */ 
fprintf(fp2,  "%f  \n",  * (all_data+2*line  data_index [i] +1)  );   /*  y-coord  */ 


f close  (fp) ; 
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f close (fp2) ; 

cfree (all_data) ; 

/*  Disconnect.  */ 
disconnect  robot  ( 1 ) ; 


void  collect_xy (float  *all_data,  int  *pt_to_no_of_all_data) 
{ 

int  i,  j; 

int  no_of_all_data; 

float  x[16],  y[16]  ; 

int  count  =  0; 

int  count2,  count3; 

int  old_steer, old_turret ; 

/*  get  sonar  readings  and  robot  configuration  */ 
GetSensorData ( ) ; 

for  (j=0;  j<16;  j++) 
{ 

/*  eliminate  sonar  reading  if  it  is  out  of  range  */ 
if  (SonarRange [ j]  !=  255) 
{ 

/*  robot_conf ig [0-1]  are  int  type  and  must  be  cast  into  float. 
Also,  they  are  in  10th  of  inch,  and  are  converted  into 
inches  by  dividing  by  10.0.   SonarRange [ ]  is  in  inches. 

robot_conf ig [2-3]  are  int  type  and  in  10th  of  degree. 
They  must  be  converted  to  float  before  passing  to  sin() 
and  cos ( )  and  into  radians. 
V 

i  =  count;     /*  to  simplify  the  following  eqs  */ 

x[i]  =  (float)  robot_config[0]  /10.0  + 

(  (float)  SonarRangefj]  +  ROBOT_RADIUS)  * 

(float)  cos ((double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  180.0) ) ; 

y[i]  =  (float)  robot_config[l]  /10.0  + 

(  (float)  SonarRange[ j]  +  ROBOT_RADIUS)  * 

(float)  sin( (double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  180.0) ) ; 

count ++; 
}   /*  if  */ 
}    /*  for  */ 

for  (j=0;  j<count;  j++) 
{ 

* (all_data+2*j)  =  x[j] ; 
* (all_data+2*j+l)  =y[j]; 
} 


/*  rotate  the  robot  by  7 . 5  degrees  */ 
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GetSensorData ( ) ; 

printf("  steer=  %10.2f   turret=  %10.2f   \n",  (float)  robot_conf ig [2] /10 . 0, 

(float)  robot_config[3] /10.0) ; 
old_steer  =  robot_conf ig [2 ] ; 
old_turret  =  robot_conf ig [3] ; 

while  ( (robot_conf ig [3] -old_steer) <72) 
{ 
vm(0,  20,  20); 

GetSensorData  () ; 

} 

vm  ( 0 ,  0 ,  0 )  ; 

st()  ; 

printf("  steer=  %10.2f   turret=  %10.2f   \n",  (float)  robot_conf ig [2] /10 . 0, 
(float)  robot_config[3] /10.0) ; 

count2  =  count;    /*  remember  where  we  left  off  */ 

for  (j=0;  j  < 1 6 ;  j++) 
{ 

/*  eliminate  sonar  reading  if  it  is  out  of  range  */ 

if  (SonarRange [ j ]  !=  255) 

i 

/*    robot_conf ig [0-1]  are  int  type  and  must  be  cast  into  float. 
Also,  they  are  in  10th  of  inch,  and  are  converted  into 
inches  by  dividing  by  10.0. 

robot_conf ig [2-3]  are  int  type  and  in  10th  of  degree. 
They  must  be  converted  to  float  before  passing  to  sin() 
and  cos  ( )  and  into  radians . 
*/ 

i  =  count  -  count2;     /*  to  simplify  the  following  eqs  */ 

x[i]  =  (float)  robot_config[0]  /10.0  + 

(  (float)  SonarRange[j]   +  ROBOT_RADIUS)  * 

(float)  cos ((double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  18  0.0)); 

y[i]  =  (float)  robot_config[l]  /10.0  + 

(  (float)  SonarRange [j]   +  ROBOT_RADIUS)  * 

(float)  sin ((double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  18  0.0)); 

count++; 
}   /*  if  */ 
}    /*  for  */ 

for  (j=count2;  j<count;  j++) 
{ 

* (all_data+2*j )  =  x [j-count2] ; 
* (all_data+2*j+l)  =  y [j-count2] ; 
} 


/*  rotate  the  robot  by  another  7.5  degrees  */ 
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GetSensorData ( )  ; 
old_steer  =  robot_conf ig [2]  ; 
old_turret  =  robot_conf ig [3] ; 
while  ( ( robot_conf ig [3 ] -old_steer ) <72 ) 
{ 

vm  ( 0 ,  2  0,  20); 

GetSensorData ( ) ; 
} 

vm ( 0 ,  0 ,  0 ) ; 

st  ( )  ; 

printf("  steer=  %10.2f   turret=  %10.2f   \n",  (float)  robot_conf ig [2] /10 . 0, 
(float)  robot_config[3] /10.0)  ; 

count3  =  count;    /*  remember  the  counter  */ 

for  (j=0;  j  < 1 6 ;  j++) 
{ 

/*  eliminate  sonar  reading  if  it  is  out  of  range  */ 
if  (SonarRange[ j]  !=  255) 
{ 

/*  robot_conf ig [0-1]  are  int  type  and  must  be  cast  into  float. 
Also,  they  are  in  10th  of  inch,  and  are  converted  into 
inches  by  dividing  by  10.0. 

robot_conf ig [2-3]  are  int  type  and  in  10th  of  degree. 
They  must  be  converted  to  float  before  passing  to  sin() 
and  cos()  and  into  radians. 
*/ 

i  =  count  -  count3;     /*  to  simplify  the  following  eqs  */ 

x[i]  =  (float)  robot_config[0]  /10.0  + 

(  (float)  SonarRangefj]   +  ROBOT_RADIUS)  * 

(float)  cos ((double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  180.0) ) ; 

y[i]  =  (float)  robot_config[l]  /10.0  + 

(  (float)  SonarRange[j]   +  ROBOT_RADIUS)  * 

(float)  sin( (double)  j  *  PI/8.0  +  (double)  robot_conf ig [3]  *  PI/ (10.0 

*  180.0) )  ; 

count++; 
}   /*  if  */ 
)    /*  for  */ 

for  (j=count3;  j<count;  j++) 
{ 

* (all_data+2*j)    =  x [ j-count3]  ; 

* (all_data+2*j+l)  =  y [j-count3] ; 
} 

/*  return  the  number  of  data  points  collected  */ 
*pt  to_no_of_all_data  =  count; 


pr int f ("Number  of  data  collected  =  %d  \n",  count) 


} 
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/*  GetSensorData ( ) .  Read  in  sensor  data  and  load  into  arrays. 

void  GetSensorData  (void) 

{ 

int  i  ; 


/*  Read  all  sensors  and  load  data  into  State  array.  */ 
gs  ( )  ; 


/*  Read  State  array  data  and  put  readings  into  individual  arrays.  */ 
for  (i  =  0;  i  <  16;  i++) 
{ 

/*  Sonar  ranges  are  given  in  inches,  and  can  be  between  6  and 

255,  inclusive.  */ 

SonarRange [i]  =  State [17+i] ; 

/*  IR  readings  are  between  0  and  15,  inclusive.  This  value  is 
inversely  proportional  to  the  light  reflected  by  the  detected 
object,  and  is  thus  proportional  to  the  distance  of  the 
object.  Due  to  the  many  environmental  variables  effecting  the 
reflectance  of  infrared  light,  distances  cannot  be  accurately 
ascribed  to  the  IR  readings.  */ 
IRRange[i]  =  State [1+i] ; 


for    (i    =    0;    i    <    4;    i++) 

robot   config[i]    =   State [34+i] ; 
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APPENDIX  D.  SYNTHETIC  DATA  INPUT  PROGRAM  (synth_data.c) 


* 

*  PROGRAM:  synth_data . c 

* 

*  PURPOSE:  To  read  an  input  file  containing  synthetic  coordinate  data 

*  simulating  a  sonar  image  for  subsequent  processing  by  the  Hough 

*  algorithm  hough. c.   The  first  line  of  the  file  must  contain  the 

*  total  number  of  points  to  be  processed. 
* 


#include  <stdio.h> 
#include  <stdlib.h> 
iinclude  <string.h> 


void  hough_transform( ) ; 

float  *all_data; 

int  *line_data_index; 

int  no_of_line_data; 

void  main(int  argc,  char  *argv[]) 
{ 

FILE  *fpl,  *fp2; 

int  i, j ,m; 

int  no_of_all_data;   /*  no.  of  input  data  points  */ 

float  *line_data; 

/* 

This  program  takes  two  file  name  arguments.   The  first  file 
contains  the  coordinates  of  input  points.  The  second  file 
contains  the  coordinates  of  points  that  fit  into  a  line. 
*/ 

if  (argc  <  3) 
{ 

printf ("Please  enter  two  file  names.  \n"); 

exit  (1)  ; 
} 


/*  open  the  input  file.  */ 

if  ((fpl  =  fopen(* (argv+1) , "rw") )  ==  NULL) 
{ 

printf ("Can' t  open  %s   \n",  * (argv+1)) 
exit (1) ; 
} 


/*  open  the  output  file.  */ 

if  ((fp2  =  fopen(* (argv+2) , "w") )  ==  NULL) 

{ 

printf ("Can't  open  %s   \n",  * (argv+2)) 
exit  (1)  ; 
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/*  read  the  input  file,  which  is  a  2-dim  array  of 
the  (x,Y)  coordinates  of  the  points. 
First,  get  the  number  of  data  points  which  is 
given  by  the  first  integer  in  the  input  file. 
Next,  allocate  memory  for  storing  the  data  points.  */ 

fscanf(fpl,  "%d",  &no_of_all_data) ; 

all_data  =  malloc (  sizeof (float)  *    2    *    no_of_all_data)  ; 

for  (i=0;  i<no_of_all_data;  i++) 

{ 

fscanf(fpl,  "%f",  all_data+2*i) ;     /*  x  coordinate  */ 
fscanf(fpl,  "%f",  all_data+2*i+l) ;   /*  y  coordinate  */ 

} 


/*  call  hough_transform( )    */ 

line_data_index  =  malloc (  sizeof (int)  *   no_of_all_data) ; 
for  (i=0;  i<no_of_all_data;  i++) 
* (line_data_index+i)  =  0; 

hough_transform(all_data,  &no_of_all_data,  line_data_index,  &no_of_line_data) ; 

/*   printf ( "Point  list  for  the  bin  with  the  most  points  from  main:  \n"); 
printf ("no  of  line  pt :  %d  \n",  no_of_line_data) ; 

for  (m=0;  m<no_of_line_data;  m++) 

printf ("%d  \n",  * (line_data_index+m) ) ; 
*/ 

printf ("Return  from  hough. c  \n"); 

/*  call  line_estimation ( )    */ 

line  estimation (all  data,  line  data  index,  &no  of  line  data); 


for  (i=0;  i<no_of_line_data;  i++) 

{ 

fprintf (fp2, "  %f    ",  * (all_data+2*line_data_index [i] ) ) ;      /*  x-coord  */ 
fprintf(fp2,  "%f  \n",  * (all_data+2*line_data_index [i] +1)  ) ;   /*  y-coord  */ 

} 

f close (fpl) ; 
fclose(fp2) ; 
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